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

added multiplyHessian in HessianFactor (not ready)

release/4.3a0
Luca Carlone 2013-10-24 22:22:29 +00:00
parent 621483bc9b
commit 6f7d4ddb50
3 changed files with 103 additions and 126 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

100
.cproject
View File

@ -1,19 +1,17 @@
<?xml version="1.0" encoding="UTF-8" standalone="no"?> <?xml version="1.0" encoding="UTF-8" standalone="no"?>
<?fileVersion 4.0.0?> <?fileVersion 4.0.0?><cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
<storageModule moduleId="org.eclipse.cdt.core.settings"> <storageModule moduleId="org.eclipse.cdt.core.settings">
<cconfiguration id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544"> <cconfiguration id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544">
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544" moduleId="org.eclipse.cdt.core.settings" name="MacOSX GCC"> <storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544" moduleId="org.eclipse.cdt.core.settings" name="MacOSX GCC">
<externalSettings/> <externalSettings/>
<extensions> <extensions>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
</extensions> </extensions>
</storageModule> </storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0"> <storageModule moduleId="cdtBuildSystem" version="4.0.0">
@ -54,13 +52,13 @@
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.1441575890" moduleId="org.eclipse.cdt.core.settings" name="Timing"> <storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.1441575890" moduleId="org.eclipse.cdt.core.settings" name="Timing">
<externalSettings/> <externalSettings/>
<extensions> <extensions>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
</extensions> </extensions>
</storageModule> </storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0"> <storageModule moduleId="cdtBuildSystem" version="4.0.0">
@ -99,13 +97,13 @@
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216" moduleId="org.eclipse.cdt.core.settings" name="fast"> <storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216" moduleId="org.eclipse.cdt.core.settings" name="fast">
<externalSettings/> <externalSettings/>
<extensions> <extensions>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
</extensions> </extensions>
</storageModule> </storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0"> <storageModule moduleId="cdtBuildSystem" version="4.0.0">
@ -293,30 +291,6 @@
<storageModule moduleId="org.eclipse.cdt.core.LanguageSettingsProviders"/> <storageModule moduleId="org.eclipse.cdt.core.LanguageSettingsProviders"/>
<storageModule moduleId="org.eclipse.cdt.make.core.buildtargets"> <storageModule moduleId="org.eclipse.cdt.make.core.buildtargets">
<buildTargets> <buildTargets>
<target name="SmartProjectionFactorExample_kitti_nonbatch.run" path="build/gtsam_unstable/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>SmartProjectionFactorExample_kitti_nonbatch.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="SmartProjectionFactorExample_kitti.run" path="build/gtsam_unstable/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>SmartProjectionFactorExample_kitti.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="SmartProjectionFactorTesting.run" path="build/gtsam_unstable/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>SmartProjectionFactorTesting.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="check" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -863,6 +837,14 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testSmartProjectionHessianFactor.run" path="build/gtsam_unstable/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSmartProjectionHessianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDiscreteFactor.run" path="build/gtsam/discrete" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testDiscreteFactor.run" path="build/gtsam/discrete" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
@ -1643,46 +1625,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="timeCalibratedCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>timeCalibratedCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPinholeCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testPinholeCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timePinholeCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>timePinholeCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCal3DS2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testCal3DS2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCal3Bundler.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testCal3Bundler.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -2027,10 +1969,10 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="SFMExample.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="VisualSLAMExample.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>SFMExample.run</buildTarget> <buildTarget>VisualSLAMExample.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
@ -2067,14 +2009,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="SFMExample_bal.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>SFMExample_bal.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testImuFactor.run" path="build-debug/gtsam_unstable/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testImuFactor.run" path="build-debug/gtsam_unstable/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments> <buildArguments>-j4</buildArguments>

98
gtsam/linear/HessianFactor.cpp
View File

@ -86,8 +86,8 @@ Scatter::Scatter(const GaussianFactorGraph& gfg, boost::optional<const Ordering&
const_iterator entry = find(key); const_iterator entry = find(key);
if(entry == end()) if(entry == end())
throw std::invalid_argument( throw std::invalid_argument(
"The ordering provided to the HessianFactor Scatter constructor\n" "The ordering provided to the HessianFactor Scatter constructor\n"
"contained extra variables that did not appear in the factors to combine."); "contained extra variables that did not appear in the factors to combine.");
at(key).slot = (slot ++); at(key).slot = (slot ++);
} }
} }
@ -102,7 +102,7 @@ Scatter::Scatter(const GaussianFactorGraph& gfg, boost::optional<const Ordering&
/* ************************************************************************* */ /* ************************************************************************* */
HessianFactor::HessianFactor() : HessianFactor::HessianFactor() :
info_(cref_list_of<1>(1)) info_(cref_list_of<1>(1))
{ {
linearTerm().setZero(); linearTerm().setZero();
constantTerm() = 0.0; constantTerm() = 0.0;
@ -110,10 +110,10 @@ 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(cref_list_of<1>(j)), info_(cref_list_of<2>(G.cols())(1)) GaussianFactor(cref_list_of<1>(j)), info_(cref_list_of<2>(G.cols())(1))
{ {
if(G.rows() != G.cols() || G.rows() != g.size()) throw invalid_argument( if(G.rows() != G.cols() || G.rows() != g.size()) 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");
info_(0,0) = G; info_(0,0) = G;
info_(0,1) = g; info_(0,1) = g;
info_(1,1)(0,0) = f; info_(1,1)(0,0) = f;
@ -123,11 +123,11 @@ 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(cref_list_of<1>(j)), GaussianFactor(cref_list_of<1>(j)),
info_(cref_list_of<2> (Sigma.cols()) (1) ) info_(cref_list_of<2> (Sigma.cols()) (1) )
{ {
if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size()) throw invalid_argument( if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size()) 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");
info_(0,0) = Sigma.inverse(); // G info_(0,0) = Sigma.inverse(); // G
info_(0,1) = info_(0,0) * mu; // g info_(0,1) = info_(0,0) * mu; // g
info_(1,1)(0,0) = mu.dot(info_(0,1).col(0)); // f info_(1,1)(0,0) = mu.dot(info_(0,1).col(0)); // f
@ -135,10 +135,10 @@ HessianFactor::HessianFactor(Key j, const Vector& mu, const Matrix& Sigma) :
/* ************************************************************************* */ /* ************************************************************************* */
HessianFactor::HessianFactor(Key j1, Key j2, HessianFactor::HessianFactor(Key j1, Key j2,
const Matrix& G11, const Matrix& G12, const Vector& g1, const Matrix& G11, const Matrix& G12, const Vector& g1,
const Matrix& G22, const Vector& g2, double f) : const Matrix& G22, const Vector& g2, double f) :
GaussianFactor(cref_list_of<2>(j1)(j2)), GaussianFactor(cref_list_of<2>(j1)(j2)),
info_(cref_list_of<3> (G11.cols()) (G22.cols()) (1) ) info_(cref_list_of<3> (G11.cols()) (G22.cols()) (1) )
{ {
info_(0,0) = G11; info_(0,0) = G11;
info_(0,1) = G12; info_(0,1) = G12;
@ -150,14 +150,14 @@ GaussianFactor(cref_list_of<2>(j1)(j2)),
/* ************************************************************************* */ /* ************************************************************************* */
HessianFactor::HessianFactor(Key j1, Key j2, Key j3, HessianFactor::HessianFactor(Key j1, Key j2, Key j3,
const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1, const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1,
const Matrix& G22, const Matrix& G23, const Vector& g2, const Matrix& G22, const Matrix& G23, const Vector& g2,
const Matrix& G33, const Vector& g3, double f) : const Matrix& G33, const Vector& g3, double f) :
GaussianFactor(cref_list_of<3>(j1)(j2)(j3)), GaussianFactor(cref_list_of<3>(j1)(j2)(j3)),
info_(cref_list_of<4> (G11.cols()) (G22.cols()) (G33.cols()) (1) ) info_(cref_list_of<4> (G11.cols()) (G22.cols()) (G33.cols()) (1) )
{ {
if(G11.rows() != G11.cols() || G11.rows() != G12.rows() || G11.rows() != G13.rows() || G11.rows() != g1.size() || if(G11.rows() != G11.cols() || G11.rows() != G12.rows() || G11.rows() != G13.rows() || G11.rows() != g1.size() ||
G22.cols() != G12.cols() || G33.cols() != G13.cols() || G22.cols() != g2.size() || G33.cols() != g3.size()) G22.cols() != G12.cols() || G33.cols() != G13.cols() || G22.cols() != g2.size() || G33.cols() != g3.size())
throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
info_(0,0) = G11; info_(0,0) = G11;
info_(0,1) = G12; info_(0,1) = G12;
@ -173,13 +173,13 @@ GaussianFactor(cref_list_of<3>(j1)(j2)(j3)),
/* ************************************************************************* */ /* ************************************************************************* */
namespace { namespace {
DenseIndex _getSizeHF(const Vector& m) { return m.size(); } DenseIndex _getSizeHF(const Vector& m) { return m.size(); }
} }
/* ************************************************************************* */ /* ************************************************************************* */
HessianFactor::HessianFactor(const std::vector<Key>& js, const std::vector<Matrix>& Gs, HessianFactor::HessianFactor(const std::vector<Key>& js, const std::vector<Matrix>& Gs,
const std::vector<Vector>& gs, double f) : const std::vector<Vector>& gs, double f) :
GaussianFactor(js), info_(br::join(gs | br::transformed(&_getSizeHF), ListOfOne((DenseIndex)1))) GaussianFactor(js), info_(br::join(gs | br::transformed(&_getSizeHF), ListOfOne((DenseIndex)1)))
{ {
// Get the number of variables // Get the number of variables
size_t variable_count = js.size(); size_t variable_count = js.size();
@ -222,33 +222,33 @@ GaussianFactor(js), info_(br::join(gs | br::transformed(&_getSizeHF), ListOfOne(
/* ************************************************************************* */ /* ************************************************************************* */
namespace { namespace {
void _FromJacobianHelper(const JacobianFactor& jf, SymmetricBlockMatrix& info) void _FromJacobianHelper(const JacobianFactor& jf, SymmetricBlockMatrix& info)
{
gttic(HessianFactor_fromJacobian);
const SharedDiagonal& jfModel = jf.get_model();
if(jfModel)
{ {
gttic(HessianFactor_fromJacobian); if(jf.get_model()->isConstrained())
const SharedDiagonal& jfModel = jf.get_model(); throw invalid_argument("Cannot construct HessianFactor from JacobianFactor with constrained noise model");
if(jfModel) info.full().noalias() = jf.matrixObject().full().transpose() *
{
if(jf.get_model()->isConstrained())
throw invalid_argument("Cannot construct HessianFactor from JacobianFactor with constrained noise model");
info.full().noalias() = jf.matrixObject().full().transpose() *
(jfModel->invsigmas().array() * jfModel->invsigmas().array()).matrix().asDiagonal() * (jfModel->invsigmas().array() * jfModel->invsigmas().array()).matrix().asDiagonal() *
jf.matrixObject().full(); jf.matrixObject().full();
} else { } else {
info.full().noalias() = jf.matrixObject().full().transpose() * jf.matrixObject().full(); info.full().noalias() = jf.matrixObject().full().transpose() * jf.matrixObject().full();
}
} }
} }
}
/* ************************************************************************* */ /* ************************************************************************* */
HessianFactor::HessianFactor(const JacobianFactor& jf) : HessianFactor::HessianFactor(const JacobianFactor& jf) :
GaussianFactor(jf), info_(SymmetricBlockMatrix::LikeActiveViewOf(jf.matrixObject())) GaussianFactor(jf), info_(SymmetricBlockMatrix::LikeActiveViewOf(jf.matrixObject()))
{ {
_FromJacobianHelper(jf, info_); _FromJacobianHelper(jf, info_);
} }
/* ************************************************************************* */ /* ************************************************************************* */
HessianFactor::HessianFactor(const GaussianFactor& gf) : HessianFactor::HessianFactor(const GaussianFactor& gf) :
GaussianFactor(gf) GaussianFactor(gf)
{ {
// Copy the matrix data depending on what type of factor we're copying from // Copy the matrix data depending on what type of factor we're copying from
if(const JacobianFactor* jf = dynamic_cast<const JacobianFactor*>(&gf)) if(const JacobianFactor* jf = dynamic_cast<const JacobianFactor*>(&gf))
@ -268,12 +268,12 @@ HessianFactor::HessianFactor(const GaussianFactor& gf) :
/* ************************************************************************* */ /* ************************************************************************* */
namespace { namespace {
DenseIndex _dimFromScatterEntry(const Scatter::value_type& key_slotentry) { DenseIndex _dimFromScatterEntry(const Scatter::value_type& key_slotentry) {
return key_slotentry.second.dimension; } } return key_slotentry.second.dimension; } }
/* ************************************************************************* */ /* ************************************************************************* */
HessianFactor::HessianFactor(const GaussianFactorGraph& factors, HessianFactor::HessianFactor(const GaussianFactorGraph& factors,
boost::optional<const Scatter&> scatter) boost::optional<const Scatter&> scatter)
{ {
boost::optional<Scatter> computedScatter; boost::optional<Scatter> computedScatter;
if(!scatter) { if(!scatter) {
@ -478,7 +478,7 @@ GaussianConditional::shared_ptr HessianFactor::splitEliminatedFactor(size_t nrFr
VerticalBlockMatrix Ab = VerticalBlockMatrix::LikeActiveViewOf(info_, varDim); VerticalBlockMatrix Ab = VerticalBlockMatrix::LikeActiveViewOf(info_, varDim);
Ab.full() = info_.range(0, nrFrontals, 0, info_.nBlocks()); Ab.full() = info_.range(0, nrFrontals, 0, info_.nBlocks());
GaussianConditional::shared_ptr conditional = boost::make_shared<GaussianConditional>( GaussianConditional::shared_ptr conditional = boost::make_shared<GaussianConditional>(
keys_, nrFrontals, Ab); keys_, nrFrontals, Ab);
gttoc(Construct_conditional); gttoc(Construct_conditional);
gttic(Remaining_factor); gttic(Remaining_factor);
@ -503,11 +503,25 @@ GaussianFactor::shared_ptr HessianFactor::negate() const
void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x,
VectorValues& y) { VectorValues& y) {
for(size_t posRow=0; posRow<size(); ++posRow){ // loops over the rows
pair<VectorValues::iterator, bool> yi = y.tryInsert(keys_[posRow], Vector());
if(yi.second)
yi.first->second = Vector::Zero(getDim(begin() + posRow));
for(size_t posCol=0; posCol<size(); ++posCol){ // loops over the columns
Vector X = x.at(keys_[posCol]);
// yi.at(keys_[posRow])// this is what we have to update
// X is the input vector
// we should select the blocks we need in (A'A)
Matrix Hij = info(begin()+posRow, begin()+posCol);
gtsam::transposeMultiplyAdd(alpha, Hij, X, yi.first->second);
}
}
} }
/* ************************************************************************* */ /* ************************************************************************* */
std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<HessianFactor> > std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<HessianFactor> >
EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys) EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys)
{ {
gttic(EliminateCholesky); gttic(EliminateCholesky);
@ -517,8 +531,8 @@ std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<HessianFacto
jointFactor = boost::make_shared<HessianFactor>(factors, Scatter(factors, keys)); jointFactor = boost::make_shared<HessianFactor>(factors, Scatter(factors, keys));
} catch(std::invalid_argument&) { } catch(std::invalid_argument&) {
throw InvalidDenseElimination( throw InvalidDenseElimination(
"EliminateCholesky was called with a request to eliminate variables that are not\n" "EliminateCholesky was called with a request to eliminate variables that are not\n"
"involved in the provided factors."); "involved in the provided factors.");
} }
// Do dense elimination // Do dense elimination
@ -533,7 +547,7 @@ std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<HessianFacto
/* ************************************************************************* */ /* ************************************************************************* */
std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<GaussianFactor> > std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<GaussianFactor> >
EliminatePreferCholesky(const GaussianFactorGraph& factors, const Ordering& keys) EliminatePreferCholesky(const GaussianFactorGraph& factors, const Ordering& keys)
{ {
gttic(EliminatePreferCholesky); gttic(EliminatePreferCholesky);

29
gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp
View File

@ -158,6 +158,15 @@ static GaussianFactorGraph createSimpleGaussianFactorGraph() {
return fg; return fg;
} }
/* ************************************************************************* */
static GaussianFactorGraph createGaussianFactorGraphWithHessianFactor() {
GaussianFactorGraph fg = createSimpleGaussianFactorGraph();
fg += HessianFactor(1, 2, 100*ones(2,2), 200*ones(2,2), (Vec(2) << 0.0, 1.0),
400*ones(2,2), (Vec(2) << 1.0, 1.0), 0.0);
return fg;
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GaussianFactorGraph, gradient ) TEST( GaussianFactorGraph, gradient )
{ {
@ -242,6 +251,26 @@ TEST( GaussianFactorGraph, multiplyHessian )
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
} }
/* ************************************************************************* */
//TEST( GaussianFactorGraph, multiplyHessian2 )
//{
// GaussianFactorGraph A = createGaussianFactorGraphWithHessianFactor();
//
// VectorValues x = map_list_of
// (0, (Vec(2) << 1,2))
// (1, (Vec(2) << 3,4))
// (2, (Vec(2) << 5,6));
//
// // expected from matlab: -450 -450 2900 2900 6750 6850
// VectorValues expected;
// expected.insert(0, (Vec(2) << -450, -450));
// expected.insert(1, (Vec(2) << 2900, 2900));
// expected.insert(2, (Vec(2) << 6750, 6850));
//
// VectorValues actual = A.multiplyHessian(x);
// EXPECT(assert_equal(expected, actual));
//}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);} int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */ /* ************************************************************************* */