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

Fixed abort due to unaligned memory allocation in SmartFactorBase

release/4.3a0
Stephen Camp 2014-07-01 08:40:27 -04:00
parent a3b001be36
commit 2eef2c14b5
7 changed files with 18 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdVector.h vendored
View File

@ -11,7 +11,8 @@
#ifndef EIGEN_STDVECTOR_H
#define EIGEN_STDVECTOR_H
#include "Eigen/src/StlSupport/details.h"
//#include "Eigen/src/StlSupport/details.h"
#include "gtsam/3rdparty/Eigen/Eigen/src/StlSupport/details.h"
/**
* This section contains a convenience MACRO which allows an easy specialization of

2
gtsam/slam/JacobianFactorQ.h
View File

@ -37,7 +37,7 @@ public:
}
/// Constructor
JacobianFactorQ(const std::vector<typename Base::KeyMatrix2D>& Fblocks,
JacobianFactorQ(const std::vector<typename Base::KeyMatrix2D, Eigen::aligned_allocator<KeyMatrix2D>>& Fblocks,
const Matrix& E, const Matrix3& P, const Vector& b,
const SharedDiagonal& model = SharedDiagonal()) :
JacobianSchurFactor<D>() {

2
gtsam/slam/JacobianFactorSVD.h
View File

@ -36,7 +36,7 @@ public:
}
/// Constructor
JacobianFactorSVD(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& Enull, const Vector& b,
JacobianFactorSVD(const std::vector<KeyMatrix2D,Eigen::aligned_allocator<KeyMatrix2D>>& Fblocks, const Matrix& Enull, const Vector& b,
const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor<D>() {
size_t numKeys = Enull.rows() / 2;
size_t j = 0, m2 = 2*numKeys-3;

15
gtsam/slam/SmartFactorBase.h
View File

@ -33,6 +33,7 @@
#include <boost/optional.hpp>
#include <boost/make_shared.hpp>
#include <vector>
#include <gtsam/3rdparty/Eigen/Eigen/StdVector>
namespace gtsam {
/// Base class with no internal point, completely functional
@ -65,6 +66,8 @@ protected:
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
@ -258,7 +261,7 @@ public:
// ****************************************************************************************************
/// Compute F, E only (called below in both vanilla and SVD versions)
/// Given a Point3, assumes dimensionality is 3
double computeJacobians(std::vector<KeyMatrix2D>& Fblocks, Matrix& E,
double computeJacobians(std::vector<KeyMatrix2D,Eigen::aligned_allocator<KeyMatrix2D>>& Fblocks, Matrix& E,
Vector& b, const Cameras& cameras, const Point3& point) const {
size_t numKeys = this->keys_.size();
@ -295,7 +298,7 @@ public:
// ****************************************************************************************************
/// Version that computes PointCov, with optional lambda parameter
double computeJacobians(std::vector<KeyMatrix2D>& Fblocks, Matrix& E,
double computeJacobians(std::vector<KeyMatrix2D,Eigen::aligned_allocator<KeyMatrix2D>>& Fblocks, Matrix& E,
Matrix3& PointCov, Vector& b, const Cameras& cameras, const Point3& point,
double lambda = 0.0, bool diagonalDamping = false) const {
@ -326,7 +329,7 @@ public:
const double lambda = 0.0) const {
size_t numKeys = this->keys_.size();
std::vector<KeyMatrix2D> Fblocks;
std::vector<KeyMatrix2D,Eigen::aligned_allocator<KeyMatrix2D>> Fblocks;
double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point,
lambda);
F = zeros(Z::Dim() * numKeys, D * numKeys);
@ -339,7 +342,7 @@ public:
// ****************************************************************************************************
/// SVD version
double computeJacobiansSVD(std::vector<KeyMatrix2D>& Fblocks, Matrix& Enull,
double computeJacobiansSVD(std::vector<KeyMatrix2D,Eigen::aligned_allocator<KeyMatrix2D>>& Fblocks, Matrix& Enull,
Vector& b, const Cameras& cameras, const Point3& point, double lambda =
0.0, bool diagonalDamping = false) const {
@ -639,7 +642,7 @@ public:
boost::shared_ptr<JacobianFactorQ<D> > createJacobianQFactor(
const Cameras& cameras, const Point3& point, double lambda = 0.0,
bool diagonalDamping = false) const {
std::vector<KeyMatrix2D> Fblocks;
std::vector<KeyMatrix2D,Eigen::aligned_allocator<KeyMatrix2D>> Fblocks;
Matrix E;
Matrix3 PointCov;
Vector b;
@ -652,7 +655,7 @@ public:
boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
const Cameras& cameras, const Point3& point, double lambda = 0.0) const {
size_t numKeys = this->keys_.size();
std::vector < KeyMatrix2D > Fblocks;
std::vector < KeyMatrix2D,Eigen::aligned_allocator<KeyMatrix2D> > Fblocks;
Vector b;
Matrix Enull(Z::Dim()*numKeys, Z::Dim()*numKeys-3);
computeJacobiansSVD(Fblocks, Enull, b, cameras, point, lambda);

2
gtsam/slam/SmartStereoProjectionFactor.h
View File

@ -43,6 +43,8 @@ protected:
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
SmartStereoProjectionFactorState() {
}
// Hessian representation (after Schur complement)

2
gtsam/slam/SmartStereoProjectionPoseFactor.h
View File

@ -47,6 +47,8 @@ protected:
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// shorthand for base class type
typedef SmartStereoProjectionFactor<POSE, LANDMARK, CALIBRATION, 6> Base;

1
gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp
View File

@ -79,6 +79,7 @@ void stereo_projectToMultipleCameras(
/* ************************************************************************* */
TEST( SmartStereoProjectionPoseFactor, Constructor) {
fprintf(stderr,"Test 1 Complete");
SmartFactor::shared_ptr factor1(new SmartFactor());
}