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

Obtain dim by using traits.

release/4.3a0
krunalchande 2014-12-19 18:05:21 -05:00 committed by thduynguyen
parent 147d174a66
commit ccc243d37a
1 changed files with 33 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
gtsam_unstable/nonlinear/NonlinearConstraint.h
View File

@ -7,6 +7,7 @@
#pragma once #pragma once
#include <gtsam/base/Manifold.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/linear/HessianFactor.h> #include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
@ -51,6 +52,9 @@ protected:
typedef NoiseModelFactor1<VALUE> Base; typedef NoiseModelFactor1<VALUE> Base;
typedef NonlinearConstraint1<VALUE> This; typedef NonlinearConstraint1<VALUE> This;
private:
static const int X1Dim = traits::dimension<VALUE>::value;
public: public:
/** /**
@ -104,7 +108,7 @@ public:
} }
return boost::make_shared<HessianFactor>(Base::key(), lG11sum, return boost::make_shared<HessianFactor>(Base::key(), lG11sum,
zero(x1.dim()), 100.0); zero(X1Dim), 100.0);
} }
/** evaluate Hessians for lambda factors */ /** evaluate Hessians for lambda factors */
@ -119,11 +123,11 @@ public:
if (debug) { if (debug) {
gtsam::print(G11all, "G11all: "); gtsam::print(G11all, "G11all: ");
std::cout << "x1dim: " << x1.dim() << std::endl; std::cout << "x1dim: " << X1Dim << std::endl;
} }
for (size_t i = 0; i < Base::get_noiseModel()->dim(); ++i) { for (size_t i = 0; i < Base::get_noiseModel()->dim(); ++i) {
G11.push_back(G11all.block(i * x1.dim(), 0, x1.dim(), x1.dim())); G11.push_back(G11all.block(i * X1Dim, 0, X1Dim, X1Dim));
if (debug) if (debug)
gtsam::print(G11[i], "G11: "); gtsam::print(G11[i], "G11: ");
} }
@ -167,6 +171,10 @@ protected:
typedef NoiseModelFactor2<VALUE1, VALUE2> Base; typedef NoiseModelFactor2<VALUE1, VALUE2> Base;
typedef NonlinearConstraint2<VALUE1, VALUE2> This; typedef NonlinearConstraint2<VALUE1, VALUE2> This;
private:
static const int X1Dim = traits::dimension<VALUE1>::value;
static const int X2Dim = traits::dimension<VALUE2>::value;
public: public:
/** /**
@ -228,7 +236,7 @@ public:
} }
return boost::make_shared<HessianFactor>(Base::keys_[0], Base::keys_[1], return boost::make_shared<HessianFactor>(Base::keys_[0], Base::keys_[1],
lG11sum, lG12sum, zero(x1.dim()), lG22sum, zero(x2.dim()), 0.0); lG11sum, lG12sum, zero(X1Dim), lG22sum, zero(X2Dim), 0.0);
} }
/** evaluate Hessians for lambda factors */ /** evaluate Hessians for lambda factors */
@ -250,20 +258,20 @@ public:
gtsam::print(G11all, "G11all: "); gtsam::print(G11all, "G11all: ");
gtsam::print(G12all, "G12all: "); gtsam::print(G12all, "G12all: ");
gtsam::print(G22all, "G22all: "); gtsam::print(G22all, "G22all: ");
std::cout << "x1dim: " << x1.dim() << std::endl; std::cout << "x1dim: " << traits::dimension<VALUE1>::value << std::endl;
std::cout << "x2dim: " << x2.dim() << std::endl; std::cout << "x2dim: " << traits::dimension<VALUE2>::value << std::endl;
} }
for (size_t i = 0; i < Base::get_noiseModel()->dim(); ++i) { for (size_t i = 0; i < Base::get_noiseModel()->dim(); ++i) {
G11.push_back(G11all.block(i * x1.dim(), 0, x1.dim(), x1.dim())); G11.push_back(G11all.block(i * X1Dim, 0, X1Dim, X1Dim));
if (debug) if (debug)
gtsam::print(G11[i], "G11: "); gtsam::print(G11[i], "G11: ");
G12.push_back(G12all.block(i * x1.dim(), 0, x1.dim(), x2.dim())); G12.push_back(G12all.block(i * X1Dim, 0, X1Dim, X2Dim));
if (debug) if (debug)
gtsam::print(G12[i], "G12: "); gtsam::print(G12[i], "G12: ");
G22.push_back(G22all.block(i * x2.dim(), 0, x2.dim(), x2.dim())); G22.push_back(G22all.block(i * X2Dim, 0, X2Dim, X2Dim));
if (debug) if (debug)
gtsam::print(G22[i], "G22: "); gtsam::print(G22[i], "G22: ");
} }
@ -317,6 +325,11 @@ protected:
typedef NoiseModelFactor3<VALUE1, VALUE2, VALUE3> Base; typedef NoiseModelFactor3<VALUE1, VALUE2, VALUE3> Base;
typedef NonlinearConstraint3<VALUE1, VALUE2, VALUE3> This; typedef NonlinearConstraint3<VALUE1, VALUE2, VALUE3> This;
private:
static const int X1Dim = traits::dimension<VALUE1>::value;
static const int X2Dim = traits::dimension<VALUE2>::value;
static const int X3Dim = traits::dimension<VALUE3>::value;
public: public:
/** /**
@ -387,8 +400,8 @@ public:
return boost::shared_ptr<HessianFactor>( return boost::shared_ptr<HessianFactor>(
new HessianFactor(Base::keys_[0], Base::keys_[1], Base::keys_[2], new HessianFactor(Base::keys_[0], Base::keys_[1], Base::keys_[2],
lG11sum, lG12sum, lG13sum, zero(x1.dim()), lG22sum, lG23sum, lG11sum, lG12sum, lG13sum, zero(X1Dim), lG22sum, lG23sum,
zero(x2.dim()), lG33sum, zero(x3.dim()), 0.0)); zero(X2Dim), lG33sum, zero(X3Dim), 0.0));
} }
/** /**
@ -470,33 +483,33 @@ public:
gtsam::print(G22all, "G22all: "); gtsam::print(G22all, "G22all: ");
gtsam::print(G23all, "G23all: "); gtsam::print(G23all, "G23all: ");
gtsam::print(G33all, "G33all: "); gtsam::print(G33all, "G33all: ");
std::cout << "x1dim: " << x1.dim() << std::endl; std::cout << "x1dim: " << X1Dim << std::endl;
std::cout << "x2dim: " << x2.dim() << std::endl; std::cout << "x2dim: " << X2Dim << std::endl;
std::cout << "x3dim: " << x3.dim() << std::endl; std::cout << "x3dim: " << X3Dim << std::endl;
} }
for (size_t i = 0; i < Base::get_noiseModel()->dim(); ++i) { for (size_t i = 0; i < Base::get_noiseModel()->dim(); ++i) {
G11.push_back(G11all.block(i * x1.dim(), 0, x1.dim(), x1.dim())); G11.push_back(G11all.block(i * X1Dim, 0, X1Dim, X1Dim));
if (debug) if (debug)
gtsam::print(G11[i], "G11: "); gtsam::print(G11[i], "G11: ");
G12.push_back(G12all.block(i * x1.dim(), 0, x1.dim(), x2.dim())); G12.push_back(G12all.block(i * X1Dim, 0, X1Dim, X2Dim));
if (debug) if (debug)
gtsam::print(G12[i], "G12: "); gtsam::print(G12[i], "G12: ");
G13.push_back(G13all.block(i * x1.dim(), 0, x1.dim(), x3.dim())); G13.push_back(G13all.block(i * X1Dim, 0, X1Dim, X3Dim));
if (debug) if (debug)
gtsam::print(G13[i], "G13: "); gtsam::print(G13[i], "G13: ");
G22.push_back(G22all.block(i * x2.dim(), 0, x2.dim(), x2.dim())); G22.push_back(G22all.block(i * X2Dim, 0, X2Dim, X2Dim));
if (debug) if (debug)
gtsam::print(G22[i], "G22: "); gtsam::print(G22[i], "G22: ");
G23.push_back(G23all.block(i * x2.dim(), 0, x2.dim(), x3.dim())); G23.push_back(G23all.block(i * X2Dim, 0, X2Dim, X3Dim));
if (debug) if (debug)
gtsam::print(G23[i], "G23: "); gtsam::print(G23[i], "G23: ");
G33.push_back(G33all.block(i * x3.dim(), 0, x3.dim(), x3.dim())); G33.push_back(G33all.block(i * X3Dim, 0, X3Dim, X3Dim));
if (debug) if (debug)
gtsam::print(G33[i], "G33: "); gtsam::print(G33[i], "G33: ");
} }