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

Some fixes for feedback reported in pull request #39

release/4.3a0
cbeall3 2014-11-19 12:51:12 -05:00
parent 1c6dc8a77d
commit 6529b793cc
4 changed files with 59 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/geometry/StereoPoint2.h
View File

@ -143,12 +143,12 @@ namespace gtsam {
} }
/** convenient function to get a Point2 from the left image */ /** convenient function to get a Point2 from the left image */
inline Point2 point2() const { Point2 point2() const {
return Point2(uL_, v_); return Point2(uL_, v_);
} }
/** convenient function to get a Point2 from the right image */ /** convenient function to get a Point2 from the right image */
inline Point2 const right(){ Point2 right() const {
return Point2(uR_, v_); return Point2(uR_, v_);
} }

2
gtsam/slam/JacobianFactorQ.h
View File

@ -49,7 +49,7 @@ public:
typedef std::pair<Key, Matrix> KeyMatrix; typedef std::pair<Key, Matrix> KeyMatrix;
std::vector < KeyMatrix > QF; std::vector < KeyMatrix > QF;
QF.reserve(m); QF.reserve(m);
// Below, we compute each 2m*D block A_j = Q_j * F_j = (2m*2) * (2*D) // Below, we compute each mZDim*D block A_j = Q_j * F_j = (mZDim*ZDim) * (Zdim*D)
BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks) BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks)
QF.push_back(KeyMatrix(it.first, Q.block(0, ZDim * j++, m2, ZDim) * it.second)); QF.push_back(KeyMatrix(it.first, Q.block(0, ZDim * j++, m2, ZDim) * it.second));
// Which is then passed to the normal JacobianFactor constructor // Which is then passed to the normal JacobianFactor constructor

106
gtsam/slam/SmartFactorBase.h
View File

@ -33,7 +33,6 @@
#include <boost/optional.hpp> #include <boost/optional.hpp>
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
#include <vector> #include <vector>
#include <gtsam/3rdparty/gtsam_eigen_includes.h>
namespace gtsam { namespace gtsam {
/// Base class with no internal point, completely functional /// Base class with no internal point, completely functional
@ -49,16 +48,16 @@ protected:
boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras) boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras)
typedef traits::dimension<Z> ZDim_t; ///< Dimension trait of measurement type static const int ZDim = traits::dimension<Z>::value; ///< Dimension trait of measurement type
/// Definitions for blocks of F /// Definitions for blocks of F
typedef Eigen::Matrix<double, ZDim_t::value, D> Matrix2D; // F typedef Eigen::Matrix<double, ZDim, D> Matrix2D; // F
typedef Eigen::Matrix<double, D, ZDim_t::value> MatrixD2; // F' typedef Eigen::Matrix<double, D, ZDim> MatrixD2; // F'
typedef std::pair<Key, Matrix2D> KeyMatrix2D; // Fblocks typedef std::pair<Key, Matrix2D> KeyMatrix2D; // Fblocks
typedef Eigen::Matrix<double, D, D> MatrixDD; // camera hessian block typedef Eigen::Matrix<double, D, D> MatrixDD; // camera hessian block
typedef Eigen::Matrix<double, ZDim_t::value, 3> Matrix23; typedef Eigen::Matrix<double, ZDim, 3> Matrix23;
typedef Eigen::Matrix<double, D, 1> VectorD; typedef Eigen::Matrix<double, D, 1> VectorD;
typedef Eigen::Matrix<double, ZDim_t::value, ZDim_t::value> Matrix2; typedef Eigen::Matrix<double, ZDim, ZDim> Matrix2;
/// shorthand for base class type /// shorthand for base class type
typedef NonlinearFactor Base; typedef NonlinearFactor Base;
@ -69,6 +68,8 @@ protected:
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// shorthand for a smart pointer to a factor /// shorthand for a smart pointer to a factor
@ -188,15 +189,15 @@ public:
// /// Calculate vector of re-projection errors, before applying noise model // /// Calculate vector of re-projection errors, before applying noise model
Vector reprojectionError(const Cameras& cameras, const Point3& point) const { Vector reprojectionError(const Cameras& cameras, const Point3& point) const {
Vector b = zero(ZDim_t::value * cameras.size()); Vector b = zero(ZDim * cameras.size());
size_t i = 0; size_t i = 0;
BOOST_FOREACH(const CAMERA& camera, cameras) { BOOST_FOREACH(const CAMERA& camera, cameras) {
const Z& zi = this->measured_.at(i); const Z& zi = this->measured_.at(i);
try { try {
Z e(camera.project(point) - zi); Z e(camera.project(point) - zi);
b[ZDim_t::value * i] = e.x(); b[ZDim * i] = e.x();
b[ZDim_t::value * i + 1] = e.y(); b[ZDim * i + 1] = e.y();
} catch (CheiralityException& e) { } catch (CheiralityException& e) {
std::cout << "Cheirality exception " << std::endl; std::cout << "Cheirality exception " << std::endl;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
@ -242,10 +243,10 @@ public:
const Point3& point) const { const Point3& point) const {
int numKeys = this->keys_.size(); int numKeys = this->keys_.size();
E = zeros(ZDim_t::value * numKeys, 3); E = zeros(ZDim * numKeys, 3);
Vector b = zero(2 * numKeys); Vector b = zero(2 * numKeys);
Matrix Ei(ZDim_t::value, 3); Matrix Ei(ZDim, 3);
for (size_t i = 0; i < this->measured_.size(); i++) { for (size_t i = 0; i < this->measured_.size(); i++) {
try { try {
cameras[i].project(point, boost::none, Ei); cameras[i].project(point, boost::none, Ei);
@ -254,7 +255,7 @@ public:
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
this->noise_.at(i)->WhitenSystem(Ei, b); this->noise_.at(i)->WhitenSystem(Ei, b);
E.block<ZDim_t::value, 3>(ZDim_t::value * i, 0) = Ei; E.block<ZDim, 3>(ZDim * i, 0) = Ei;
} }
// Matrix PointCov; // Matrix PointCov;
@ -268,11 +269,11 @@ public:
Vector& b, const Cameras& cameras, const Point3& point) const { Vector& b, const Cameras& cameras, const Point3& point) const {
size_t numKeys = this->keys_.size(); size_t numKeys = this->keys_.size();
E = zeros(Z::Dim() * numKeys, 3); E = zeros(ZDim * numKeys, 3);
b = zero(Z::Dim() * numKeys); b = zero(ZDim * numKeys);
double f = 0; double f = 0;
Matrix Fi(Z::Dim(), 6), Ei(Z::Dim(), 3), Hcali(Z::Dim(), D - 6), Hcam(Z::Dim(), D); Matrix Fi(ZDim, 6), Ei(ZDim, 3), Hcali(ZDim, D - 6), Hcam(ZDim, D);
for (size_t i = 0; i < this->measured_.size(); i++) { for (size_t i = 0; i < this->measured_.size(); i++) {
Vector bi; Vector bi;
@ -294,12 +295,12 @@ public:
if (D == 6) { // optimize only camera pose if (D == 6) { // optimize only camera pose
Fblocks.push_back(KeyMatrix2D(this->keys_[i], Fi)); Fblocks.push_back(KeyMatrix2D(this->keys_[i], Fi));
} else { } else {
Hcam.block<ZDim_t::value, 6>(0, 0) = Fi; // Z::Dim() x 6 block for the cameras Hcam.block<ZDim, 6>(0, 0) = Fi; // ZDim x 6 block for the cameras
Hcam.block<ZDim_t::value, D - 6>(0, 6) = Hcali; // Z::Dim() x nrCal block for the cameras Hcam.block<ZDim, D - 6>(0, 6) = Hcali; // ZDim x nrCal block for the cameras
Fblocks.push_back(KeyMatrix2D(this->keys_[i], Hcam)); Fblocks.push_back(KeyMatrix2D(this->keys_[i], Hcam));
} }
E.block<ZDim_t::value, 3>(ZDim_t::value * i, 0) = Ei; E.block<ZDim, 3>(ZDim * i, 0) = Ei;
subInsert(b, bi, Z::Dim() * i); subInsert(b, bi, ZDim * i);
} }
return f; return f;
} }
@ -340,10 +341,10 @@ public:
std::vector<KeyMatrix2D> Fblocks; std::vector<KeyMatrix2D> Fblocks;
double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point,
lambda); lambda);
F = zeros(Z::Dim() * numKeys, D * numKeys); F = zeros(This::ZDim * numKeys, D * numKeys);
for (size_t i = 0; i < this->keys_.size(); ++i) { for (size_t i = 0; i < this->keys_.size(); ++i) {
F.block<ZDim_t::value, D>(ZDim_t::value * i, D * i) = Fblocks.at(i).second; // Z::Dim() x 6 block for the cameras F.block<This::ZDim, D>(This::ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras
} }
return f; return f;
} }
@ -362,9 +363,9 @@ public:
// Do SVD on A // Do SVD on A
Eigen::JacobiSVD<Matrix> svd(E, Eigen::ComputeFullU); Eigen::JacobiSVD<Matrix> svd(E, Eigen::ComputeFullU);
Vector s = svd.singularValues(); Vector s = svd.singularValues();
// Enull = zeros(Z::Dim() * numKeys, Z::Dim() * numKeys - 3); // Enull = zeros(ZDim * numKeys, ZDim * numKeys - 3);
size_t numKeys = this->keys_.size(); size_t numKeys = this->keys_.size();
Enull = svd.matrixU().block(0, 3, Z::Dim() * numKeys, Z::Dim() * numKeys - 3); // last Z::Dim()m-3 columns Enull = svd.matrixU().block(0, 3, ZDim * numKeys, ZDim * numKeys - 3); // last ZDimm-3 columns
return f; return f;
} }
@ -378,11 +379,11 @@ public:
int numKeys = this->keys_.size(); int numKeys = this->keys_.size();
std::vector<KeyMatrix2D> Fblocks; std::vector<KeyMatrix2D> Fblocks;
double f = computeJacobiansSVD(Fblocks, Enull, b, cameras, point); double f = computeJacobiansSVD(Fblocks, Enull, b, cameras, point);
F.resize(Z::Dim() * numKeys, D * numKeys); F.resize(ZDim * numKeys, D * numKeys);
F.setZero(); F.setZero();
for (size_t i = 0; i < this->keys_.size(); ++i) for (size_t i = 0; i < this->keys_.size(); ++i)
F.block<Z::Dim(), D>(Z::Dim() * i, D * i) = Fblocks.at(i).second; // Z::Dim() x 6 block for the cameras F.block<ZDim, D>(ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras
return f; return f;
} }
@ -443,9 +444,9 @@ public:
int numKeys = this->keys_.size(); int numKeys = this->keys_.size();
/// Compute Full F ???? /// Compute Full F ????
Matrix F = zeros(Z::Dim() * numKeys, D * numKeys); Matrix F = zeros(ZDim * numKeys, D * numKeys);
for (size_t i = 0; i < this->keys_.size(); ++i) for (size_t i = 0; i < this->keys_.size(); ++i)
F.block<Z::Dim(), D>(Z::Dim() * i, D * i) = Fblocks.at(i).second; // Z::Dim() x 6 block for the cameras F.block<ZDim, D>(ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras
Matrix H(D * numKeys, D * numKeys); Matrix H(D * numKeys, D * numKeys);
Vector gs_vector; Vector gs_vector;
@ -483,16 +484,16 @@ public:
for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera
const Matrix2D& Fi1 = Fblocks.at(i1).second; const Matrix2D& Fi1 = Fblocks.at(i1).second;
const Matrix23 Ei1_P = E.block<Z::Dim(), 3>(Z::Dim() * i1, 0) * P; const Matrix23 Ei1_P = E.block<ZDim, 3>(ZDim * i1, 0) * P;
// D = (Dx2) * (2) // D = (Dx2) * (2)
// (augmentedHessian.matrix()).block<D,1> (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b // (augmentedHessian.matrix()).block<D,1> (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b
augmentedHessian(i1, numKeys) = Fi1.transpose() * b.segment<Z::Dim()>(Z::Dim() * i1) // F' * b augmentedHessian(i1, numKeys) = Fi1.transpose() * b.segment<ZDim>(ZDim * i1) // F' * b
- Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZ::Dim()) * (Z::Dim()x3) * (3*Z::Dim()m) * (Z::Dim()m x 1) - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1)
// (DxD) = (DxZ::Dim()) * ( (Z::Dim()xD) - (Z::Dim()x3) * (3xZ::Dim()) * (Z::Dim()xD) ) // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
augmentedHessian(i1, i1) = Fi1.transpose() augmentedHessian(i1, i1) = Fi1.transpose()
* (Fi1 - Ei1_P * E.block<Z::Dim(), 3>(Z::Dim() * i1, 0).transpose() * Fi1); * (Fi1 - Ei1_P * E.block<ZDim, 3>(ZDim * i1, 0).transpose() * Fi1);
// upper triangular part of the hessian // upper triangular part of the hessian
for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera
@ -500,7 +501,7 @@ public:
// (DxD) = (Dx2) * ( (2x2) * (2xD) ) // (DxD) = (Dx2) * ( (2x2) * (2xD) )
augmentedHessian(i1, i2) = -Fi1.transpose() augmentedHessian(i1, i2) = -Fi1.transpose()
* (Ei1_P * E.block<Z::Dim(), 3>(Z::Dim() * i2, 0).transpose() * Fi2); * (Ei1_P * E.block<ZDim, 3>(ZDim * i2, 0).transpose() * Fi2);
} }
} // end of for over cameras } // end of for over cameras
} }
@ -527,16 +528,16 @@ public:
// X X X X 14 // X X X X 14
const Matrix2D& Fi1 = Fblocks.at(i1).second; const Matrix2D& Fi1 = Fblocks.at(i1).second;
const Matrix23 Ei1_P = E.block<Z::Dim(), 3>(Z::Dim() * i1, 0) * P; const Matrix23 Ei1_P = E.block<ZDim, 3>(ZDim * i1, 0) * P;
{ // for i1 = i2 { // for i1 = i2
// D = (Dx2) * (2) // D = (Dx2) * (2)
gs.at(i1) = Fi1.transpose() * b.segment<Z::Dim()>(Z::Dim() * i1) // F' * b gs.at(i1) = Fi1.transpose() * b.segment<ZDim>(ZDim * i1) // F' * b
-Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZ::Dim()) * (Z::Dim()x3) * (3*Z::Dim()m) * (Z::Dim()m x 1) -Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1)
// (DxD) = (DxZ::Dim()) * ( (Z::Dim()xD) - (Z::Dim()x3) * (3xZ::Dim()) * (Z::Dim()xD) ) // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
Gs.at(GsIndex) = Fi1.transpose() Gs.at(GsIndex) = Fi1.transpose()
* (Fi1 - Ei1_P * E.block<Z::Dim(), 3>(Z::Dim() * i1, 0).transpose() * Fi1); * (Fi1 - Ei1_P * E.block<ZDim, 3>(ZDim * i1, 0).transpose() * Fi1);
GsIndex++; GsIndex++;
} }
// upper triangular part of the hessian // upper triangular part of the hessian
@ -545,7 +546,7 @@ public:
// (DxD) = (Dx2) * ( (2x2) * (2xD) ) // (DxD) = (Dx2) * ( (2x2) * (2xD) )
Gs.at(GsIndex) = -Fi1.transpose() Gs.at(GsIndex) = -Fi1.transpose()
* (Ei1_P * E.block<Z::Dim(), 3>(Z::Dim() * i2, 0).transpose() * Fi2); * (Ei1_P * E.block<ZDim, 3>(ZDim * i2, 0).transpose() * Fi2);
GsIndex++; GsIndex++;
} }
} // end of for over cameras } // end of for over cameras
@ -593,9 +594,9 @@ public:
for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera in the current factor for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera in the current factor
const Matrix2D& Fi1 = Fblocks.at(i1).second; const Matrix2D& Fi1 = Fblocks.at(i1).second;
const Matrix23 Ei1_P = E.block<Z::Dim(), 3>(Z::Dim() * i1, 0) * P; const Matrix23 Ei1_P = E.block<ZDim, 3>(ZDim * i1, 0) * P;
// D = (DxZ::Dim()) * (Z::Dim()) // D = (DxZDim) * (ZDim)
// allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7) // allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7)
// we should map those to a slot in the local (grouped) hessian (0,1,2,3,4) // we should map those to a slot in the local (grouped) hessian (0,1,2,3,4)
// Key cameraKey_i1 = this->keys_[i1]; // Key cameraKey_i1 = this->keys_[i1];
@ -605,15 +606,15 @@ public:
// vectorBlock = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal(); // vectorBlock = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal();
// add contribution of current factor // add contribution of current factor
augmentedHessian(aug_i1, aug_numKeys) = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal() augmentedHessian(aug_i1, aug_numKeys) = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal()
+ Fi1.transpose() * b.segment<Z::Dim()>(Z::Dim() * i1) // F' * b + Fi1.transpose() * b.segment<ZDim>(ZDim * i1) // F' * b
- Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZ::Dim()) * (Z::Dim()x3) * (3*Z::Dim()m) * (Z::Dim()m x 1) - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1)
// (DxD) = (DxZ::Dim()) * ( (Z::Dim()xD) - (Z::Dim()x3) * (3xZ::Dim()) * (Z::Dim()xD) ) // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
// main block diagonal - store previous block // main block diagonal - store previous block
matrixBlock = augmentedHessian(aug_i1, aug_i1); matrixBlock = augmentedHessian(aug_i1, aug_i1);
// add contribution of current factor // add contribution of current factor
augmentedHessian(aug_i1, aug_i1) = matrixBlock + augmentedHessian(aug_i1, aug_i1) = matrixBlock +
( Fi1.transpose() * (Fi1 - Ei1_P * E.block<Z::Dim(), 3>(Z::Dim() * i1, 0).transpose() * Fi1) ); ( Fi1.transpose() * (Fi1 - Ei1_P * E.block<ZDim, 3>(ZDim * i1, 0).transpose() * Fi1) );
// upper triangular part of the hessian // upper triangular part of the hessian
for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera
@ -622,12 +623,12 @@ public:
//Key cameraKey_i2 = this->keys_[i2]; //Key cameraKey_i2 = this->keys_[i2];
DenseIndex aug_i2 = KeySlotMap[this->keys_[i2]]; DenseIndex aug_i2 = KeySlotMap[this->keys_[i2]];
// (DxD) = (DxZ::Dim()) * ( (Z::Dim()xZ::Dim()) * (Z::Dim()xD) ) // (DxD) = (DxZDim) * ( (ZDimxZDim) * (ZDimxD) )
// off diagonal block - store previous block // off diagonal block - store previous block
// matrixBlock = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal(); // matrixBlock = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal();
// add contribution of current factor // add contribution of current factor
augmentedHessian(aug_i1, aug_i2) = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal() augmentedHessian(aug_i1, aug_i2) = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal()
- Fi1.transpose() * (Ei1_P * E.block<Z::Dim(), 3>(Z::Dim() * i2, 0).transpose() * Fi2); - Fi1.transpose() * (Ei1_P * E.block<ZDim, 3>(ZDim * i2, 0).transpose() * Fi2);
} }
} // end of for over cameras } // end of for over cameras
@ -647,7 +648,7 @@ public:
} }
// **************************************************************************************************** // ****************************************************************************************************
boost::shared_ptr<JacobianFactorQ<D, ZDim_t::value> > createJacobianQFactor( boost::shared_ptr<JacobianFactorQ<D, ZDim> > createJacobianQFactor(
const Cameras& cameras, const Point3& point, double lambda = 0.0, const Cameras& cameras, const Point3& point, double lambda = 0.0,
bool diagonalDamping = false) const { bool diagonalDamping = false) const {
std::vector<KeyMatrix2D> Fblocks; std::vector<KeyMatrix2D> Fblocks;
@ -656,7 +657,7 @@ public:
Vector b; Vector b;
computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda,
diagonalDamping); diagonalDamping);
return boost::make_shared<JacobianFactorQ<D, ZDim_t::value> >(Fblocks, E, PointCov, b); return boost::make_shared<JacobianFactorQ<D, ZDim> >(Fblocks, E, PointCov, b);
} }
// **************************************************************************************************** // ****************************************************************************************************
@ -665,9 +666,9 @@ public:
size_t numKeys = this->keys_.size(); size_t numKeys = this->keys_.size();
std::vector<KeyMatrix2D> Fblocks; std::vector<KeyMatrix2D> Fblocks;
Vector b; Vector b;
Matrix Enull(Z::Dim()*numKeys, Z::Dim()*numKeys-3); Matrix Enull(ZDim*numKeys, ZDim*numKeys-3);
computeJacobiansSVD(Fblocks, Enull, b, cameras, point, lambda); computeJacobiansSVD(Fblocks, Enull, b, cameras, point, lambda);
return boost::make_shared< JacobianFactorSVD<6, ZDim_t::value> >(Fblocks, Enull, b); return boost::make_shared< JacobianFactorSVD<6, ZDim> >(Fblocks, Enull, b);
} }
private: private:
@ -682,4 +683,7 @@ private:
} }
}; };
template<class POSE, class Z, class CAMERA, size_t D>
const int SmartFactorBase<POSE, Z, CAMERA, D>::ZDim;
} // \ namespace gtsam } // \ namespace gtsam

2
gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp
View File

@ -35,7 +35,7 @@
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/slam/dataset.h> #include <gtsam/slam/dataset.h>
#include <gtsam/slam/SmartStereoProjectionPoseFactor.h> #include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
#include <string> #include <string>
#include <fstream> #include <fstream>