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

Fix memory alignment issues

release/4.3a0
Sean Bowman 2017-05-13 19:35:05 -04:00
parent 6b6a8495bb
commit b04c0bb15d
31 changed files with 167 additions and 129 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
gtsam/geometry/CalibratedCamera.h
View File

@ -55,6 +55,7 @@ public:
* and this typedef informs those classes what "project" returns. * and this typedef informs those classes what "project" returns.
*/ */
typedef Point2 Measurement; typedef Point2 Measurement;
typedef Point2Vector MeasurementVector;
private: private:

18
gtsam/geometry/CameraSet.h
View File

@ -31,7 +31,7 @@ namespace gtsam {
* @brief A set of cameras, all with their own calibration * @brief A set of cameras, all with their own calibration
*/ */
template<class CAMERA> template<class CAMERA>
class CameraSet: public std::vector<CAMERA> { class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA> > {
protected: protected:
@ -40,13 +40,14 @@ protected:
* The order is kept the same as the keys that we use to create the factor. * The order is kept the same as the keys that we use to create the factor.
*/ */
typedef typename CAMERA::Measurement Z; typedef typename CAMERA::Measurement Z;
typedef typename CAMERA::MeasurementVector ZVector;
static const int D = traits<CAMERA>::dimension; ///< Camera dimension static const int D = traits<CAMERA>::dimension; ///< Camera dimension
static const int ZDim = traits<Z>::dimension; ///< Measurement dimension static const int ZDim = traits<Z>::dimension; ///< Measurement dimension
/// Make a vector of re-projection errors /// Make a vector of re-projection errors
static Vector ErrorVector(const std::vector<Z>& predicted, static Vector ErrorVector(const ZVector& predicted,
const std::vector<Z>& measured) { const ZVector& measured) {
// Check size // Check size
size_t m = predicted.size(); size_t m = predicted.size();
@ -69,7 +70,7 @@ public:
/// Definitions for blocks of F /// Definitions for blocks of F
typedef Eigen::Matrix<double, ZDim, D> MatrixZD; typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
typedef std::vector<MatrixZD> FBlocks; typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks;
/** /**
* print * print
@ -102,7 +103,7 @@ public:
* throws CheiralityException * throws CheiralityException
*/ */
template<class POINT> template<class POINT>
std::vector<Z> project2(const POINT& point, // ZVector project2(const POINT& point, //
boost::optional<FBlocks&> Fs = boost::none, // boost::optional<FBlocks&> Fs = boost::none, //
boost::optional<Matrix&> E = boost::none) const { boost::optional<Matrix&> E = boost::none) const {
@ -110,7 +111,7 @@ public:
// Allocate result // Allocate result
size_t m = this->size(); size_t m = this->size();
std::vector<Z> z; ZVector z;
z.reserve(m); z.reserve(m);
// Allocate derivatives // Allocate derivatives
@ -131,7 +132,7 @@ public:
/// Calculate vector [project2(point)-z] of re-projection errors /// Calculate vector [project2(point)-z] of re-projection errors
template<class POINT> template<class POINT>
Vector reprojectionError(const POINT& point, const std::vector<Z>& measured, Vector reprojectionError(const POINT& point, const ZVector& measured,
boost::optional<FBlocks&> Fs = boost::none, // boost::optional<FBlocks&> Fs = boost::none, //
boost::optional<Matrix&> E = boost::none) const { boost::optional<Matrix&> E = boost::none) const {
return ErrorVector(project2(point, Fs, E), measured); return ErrorVector(project2(point, Fs, E), measured);
@ -315,6 +316,9 @@ private:
void serialize(ARCHIVE & ar, const unsigned int /*version*/) { void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & (*this); ar & (*this);
} }
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
template<class CAMERA> template<class CAMERA>

1
gtsam/geometry/PinholeCamera.h
View File

@ -39,6 +39,7 @@ public:
* and this typedef informs those classes what "project" returns. * and this typedef informs those classes what "project" returns.
*/ */
typedef Point2 Measurement; typedef Point2 Measurement;
typedef Point2Vector MeasurementVector;
private: private:

2
gtsam/geometry/PinholeSet.h
View File

@ -58,7 +58,7 @@ public:
/// triangulateSafe /// triangulateSafe
TriangulationResult triangulateSafe( TriangulationResult triangulateSafe(
const std::vector<Point2>& measured, const typename CAMERA::MeasurementVector& measured,
const TriangulationParameters& params) const { const TriangulationParameters& params) const {
return gtsam::triangulateSafe(*this, measured, params); return gtsam::triangulateSafe(*this, measured, params);
} }

2
gtsam/geometry/Point2.h
View File

@ -164,7 +164,7 @@ typedef std::pair<Point2, Point2> Point2Pair;
std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p); std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p);
// For MATLAB wrapper // For MATLAB wrapper
typedef std::vector<Point2> Point2Vector; typedef std::vector<Point2, Eigen::aligned_allocator<Point2> > Point2Vector;
/// multiply with scalar /// multiply with scalar
inline Point2 operator*(double s, const Point2& p) { inline Point2 operator*(double s, const Point2& p) {

3
gtsam/geometry/Pose3.h
View File

@ -327,6 +327,9 @@ public:
} }
/// @} /// @}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
// Pose3 class // Pose3 class

3
gtsam/geometry/Rot3.h
View File

@ -508,6 +508,9 @@ namespace gtsam {
#endif #endif
} }
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
/** /**

1
gtsam/geometry/StereoCamera.h
View File

@ -43,6 +43,7 @@ public:
* and this typedef informs those classes what "project" returns. * and this typedef informs those classes what "project" returns.
*/ */
typedef StereoPoint2 Measurement; typedef StereoPoint2 Measurement;
typedef StereoPoint2Vector MeasurementVector;
private: private:
Pose3 leftCamPose_; Pose3 leftCamPose_;

2
gtsam/geometry/StereoPoint2.h
View File

@ -159,6 +159,8 @@ private:
}; };
typedef std::vector<StereoPoint2> StereoPoint2Vector;
template<> template<>
struct traits<StereoPoint2> : public internal::VectorSpace<StereoPoint2> {}; struct traits<StereoPoint2> : public internal::VectorSpace<StereoPoint2> {};

4
gtsam/geometry/tests/testCameraSet.cpp
View File

@ -32,7 +32,7 @@ using namespace gtsam;
TEST(CameraSet, Pinhole) { TEST(CameraSet, Pinhole) {
typedef PinholeCamera<Cal3Bundler> Camera; typedef PinholeCamera<Cal3Bundler> Camera;
typedef CameraSet<Camera> Set; typedef CameraSet<Camera> Set;
typedef vector<Point2> ZZ; typedef Point2Vector ZZ;
Set set; Set set;
Camera camera; Camera camera;
set.push_back(camera); set.push_back(camera);
@ -135,8 +135,8 @@ TEST(CameraSet, Pinhole) {
/* ************************************************************************* */ /* ************************************************************************* */
#include <gtsam/geometry/StereoCamera.h> #include <gtsam/geometry/StereoCamera.h>
TEST(CameraSet, Stereo) { TEST(CameraSet, Stereo) {
typedef vector<StereoPoint2> ZZ;
CameraSet<StereoCamera> set; CameraSet<StereoCamera> set;
typedef StereoCamera::MeasurementVector ZZ;
StereoCamera camera; StereoCamera camera;
set.push_back(camera); set.push_back(camera);
set.push_back(camera); set.push_back(camera);

4
gtsam/geometry/tests/testPinholeSet.cpp
View File

@ -28,7 +28,7 @@ using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
#include <gtsam/geometry/CalibratedCamera.h> #include <gtsam/geometry/CalibratedCamera.h>
TEST(PinholeSet, Stereo) { TEST(PinholeSet, Stereo) {
typedef vector<Point2> ZZ; typedef Point2Vector ZZ;
PinholeSet<CalibratedCamera> set; PinholeSet<CalibratedCamera> set;
CalibratedCamera camera; CalibratedCamera camera;
set.push_back(camera); set.push_back(camera);
@ -72,7 +72,7 @@ TEST(PinholeSet, Stereo) {
#include <gtsam/geometry/Cal3Bundler.h> #include <gtsam/geometry/Cal3Bundler.h>
TEST(PinholeSet, Pinhole) { TEST(PinholeSet, Pinhole) {
typedef PinholeCamera<Cal3Bundler> Camera; typedef PinholeCamera<Cal3Bundler> Camera;
typedef vector<Point2> ZZ; typedef Point2Vector ZZ;
PinholeSet<Camera> set; PinholeSet<Camera> set;
Camera camera; Camera camera;
set.push_back(camera); set.push_back(camera);

4
gtsam/geometry/tests/testPose2.cpp
View File

@ -743,7 +743,7 @@ namespace {
/* ************************************************************************* */ /* ************************************************************************* */
struct Triangle { size_t i_,j_,k_;}; struct Triangle { size_t i_,j_,k_;};
boost::optional<Pose2> align2(const vector<Point2>& ps, const vector<Point2>& qs, boost::optional<Pose2> align2(const Point2Vector& ps, const Point2Vector& qs,
const pair<Triangle, Triangle>& trianglePair) { const pair<Triangle, Triangle>& trianglePair) {
const Triangle& t1 = trianglePair.first, t2 = trianglePair.second; const Triangle& t1 = trianglePair.first, t2 = trianglePair.second;
vector<Point2Pair> correspondences; vector<Point2Pair> correspondences;
@ -755,7 +755,7 @@ namespace {
TEST(Pose2, align_4) { TEST(Pose2, align_4) {
using namespace align_3; using namespace align_3;
vector<Point2> ps,qs; Point2Vector ps,qs;
ps += p1, p2, p3; ps += p1, p2, p3;
qs += q3, q1, q2; // note in 3,1,2 order ! qs += q3, q1, q2; // note in 3,1,2 order !

20
gtsam/geometry/tests/testTriangulation.cpp
View File

@ -60,7 +60,7 @@ Point2 z2 = camera2.project(landmark);
TEST( triangulation, twoPoses) { TEST( triangulation, twoPoses) {
vector<Pose3> poses; vector<Pose3> poses;
vector<Point2> measurements; Point2Vector measurements;
poses += pose1, pose2; poses += pose1, pose2;
measurements += z1, z2; measurements += z1, z2;
@ -108,7 +108,7 @@ TEST( triangulation, twoPosesBundler) {
Point2 z2 = camera2.project(landmark); Point2 z2 = camera2.project(landmark);
vector<Pose3> poses; vector<Pose3> poses;
vector<Point2> measurements; Point2Vector measurements;
poses += pose1, pose2; poses += pose1, pose2;
measurements += z1, z2; measurements += z1, z2;
@ -132,7 +132,7 @@ TEST( triangulation, twoPosesBundler) {
//****************************************************************************** //******************************************************************************
TEST( triangulation, fourPoses) { TEST( triangulation, fourPoses) {
vector<Pose3> poses; vector<Pose3> poses;
vector<Point2> measurements; Point2Vector measurements;
poses += pose1, pose2; poses += pose1, pose2;
measurements += z1, z2; measurements += z1, z2;
@ -195,8 +195,8 @@ TEST( triangulation, fourPoses_distinct_Ks) {
Point2 z1 = camera1.project(landmark); Point2 z1 = camera1.project(landmark);
Point2 z2 = camera2.project(landmark); Point2 z2 = camera2.project(landmark);
vector<SimpleCamera> cameras; CameraSet<SimpleCamera> cameras;
vector<Point2> measurements; Point2Vector measurements;
cameras += camera1, camera2; cameras += camera1, camera2;
measurements += z1, z2; measurements += z1, z2;
@ -260,8 +260,8 @@ TEST( triangulation, outliersAndFarLandmarks) {
Point2 z1 = camera1.project(landmark); Point2 z1 = camera1.project(landmark);
Point2 z2 = camera2.project(landmark); Point2 z2 = camera2.project(landmark);
vector<SimpleCamera> cameras; CameraSet<SimpleCamera> cameras;
vector<Point2> measurements; Point2Vector measurements;
cameras += camera1, camera2; cameras += camera1, camera2;
measurements += z1, z2; measurements += z1, z2;
@ -308,7 +308,7 @@ TEST( triangulation, twoIdenticalPoses) {
Point2 z1 = camera1.project(landmark); Point2 z1 = camera1.project(landmark);
vector<Pose3> poses; vector<Pose3> poses;
vector<Point2> measurements; Point2Vector measurements;
poses += pose1, pose1; poses += pose1, pose1;
measurements += z1, z1; measurements += z1, z1;
@ -323,7 +323,7 @@ TEST( triangulation, onePose) {
// because there's only one camera observation // because there's only one camera observation
vector<Pose3> poses; vector<Pose3> poses;
vector<Point2> measurements; Point2Vector measurements;
poses += Pose3(); poses += Pose3();
measurements += Point2(0,0); measurements += Point2(0,0);
@ -354,7 +354,7 @@ TEST( triangulation, StereotriangulateNonlinear ) {
cameras.push_back(StereoCamera(Pose3(m1), stereoK)); cameras.push_back(StereoCamera(Pose3(m1), stereoK));
cameras.push_back(StereoCamera(Pose3(m2), stereoK)); cameras.push_back(StereoCamera(Pose3(m2), stereoK));
vector<StereoPoint2> measurements; StereoPoint2Vector measurements;
measurements += StereoPoint2(226.936, 175.212, 424.469); measurements += StereoPoint2(226.936, 175.212, 424.469);
measurements += StereoPoint2(339.571, 285.547, 669.973); measurements += StereoPoint2(339.571, 285.547, 669.973);

4
gtsam/geometry/triangulation.cpp
View File

@ -25,7 +25,7 @@ namespace gtsam {
Vector4 triangulateHomogeneousDLT( Vector4 triangulateHomogeneousDLT(
const std::vector<Matrix34>& projection_matrices, const std::vector<Matrix34>& projection_matrices,
const std::vector<Point2>& measurements, double rank_tol) { const Point2Vector& measurements, double rank_tol) {
// number of cameras // number of cameras
size_t m = projection_matrices.size(); size_t m = projection_matrices.size();
@ -54,7 +54,7 @@ Vector4 triangulateHomogeneousDLT(
} }
Point3 triangulateDLT(const std::vector<Matrix34>& projection_matrices, Point3 triangulateDLT(const std::vector<Matrix34>& projection_matrices,
const std::vector<Point2>& measurements, double rank_tol) { const Point2Vector& measurements, double rank_tol) {
Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol); Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol);

40
gtsam/geometry/triangulation.h
View File

@ -19,6 +19,7 @@
#pragma once #pragma once
#include <gtsam/geometry/PinholeCamera.h> #include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/CameraSet.h>
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/slam/TriangulationFactor.h> #include <gtsam/slam/TriangulationFactor.h>
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
@ -53,7 +54,7 @@ public:
*/ */
GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
const std::vector<Matrix34>& projection_matrices, const std::vector<Matrix34>& projection_matrices,
const std::vector<Point2>& measurements, double rank_tol = 1e-9); const Point2Vector& measurements, double rank_tol = 1e-9);
/** /**
* DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312 * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312
@ -64,7 +65,8 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
*/ */
GTSAM_EXPORT Point3 triangulateDLT( GTSAM_EXPORT Point3 triangulateDLT(
const std::vector<Matrix34>& projection_matrices, const std::vector<Matrix34>& projection_matrices,
const std::vector<Point2>& measurements, double rank_tol = 1e-9); const Point2Vector& measurements,
double rank_tol = 1e-9);
/** /**
* Create a factor graph with projection factors from poses and one calibration * Create a factor graph with projection factors from poses and one calibration
@ -78,7 +80,7 @@ GTSAM_EXPORT Point3 triangulateDLT(
template<class CALIBRATION> template<class CALIBRATION>
std::pair<NonlinearFactorGraph, Values> triangulationGraph( std::pair<NonlinearFactorGraph, Values> triangulationGraph(
const std::vector<Pose3>& poses, boost::shared_ptr<CALIBRATION> sharedCal, const std::vector<Pose3>& poses, boost::shared_ptr<CALIBRATION> sharedCal,
const std::vector<Point2>& measurements, Key landmarkKey, const Point2Vector& measurements, Key landmarkKey,
const Point3& initialEstimate) { const Point3& initialEstimate) {
Values values; Values values;
values.insert(landmarkKey, initialEstimate); // Initial landmark value values.insert(landmarkKey, initialEstimate); // Initial landmark value
@ -106,8 +108,8 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
*/ */
template<class CAMERA> template<class CAMERA>
std::pair<NonlinearFactorGraph, Values> triangulationGraph( std::pair<NonlinearFactorGraph, Values> triangulationGraph(
const std::vector<CAMERA>& cameras, const CameraSet<CAMERA>& cameras,
const std::vector<typename CAMERA::Measurement>& measurements, Key landmarkKey, const typename CAMERA::MeasurementVector& measurements, Key landmarkKey,
const Point3& initialEstimate) { const Point3& initialEstimate) {
Values values; Values values;
values.insert(landmarkKey, initialEstimate); // Initial landmark value values.insert(landmarkKey, initialEstimate); // Initial landmark value
@ -125,8 +127,8 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
/// PinholeCamera specific version // TODO: (chris) why does this exist? /// PinholeCamera specific version // TODO: (chris) why does this exist?
template<class CALIBRATION> template<class CALIBRATION>
std::pair<NonlinearFactorGraph, Values> triangulationGraph( std::pair<NonlinearFactorGraph, Values> triangulationGraph(
const std::vector<PinholeCamera<CALIBRATION> >& cameras, const CameraSet<PinholeCamera<CALIBRATION> >& cameras,
const std::vector<Point2>& measurements, Key landmarkKey, const Point2Vector& measurements, Key landmarkKey,
const Point3& initialEstimate) { const Point3& initialEstimate) {
return triangulationGraph<PinholeCamera<CALIBRATION> > // return triangulationGraph<PinholeCamera<CALIBRATION> > //
(cameras, measurements, landmarkKey, initialEstimate); (cameras, measurements, landmarkKey, initialEstimate);
@ -153,7 +155,7 @@ GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph,
template<class CALIBRATION> template<class CALIBRATION>
Point3 triangulateNonlinear(const std::vector<Pose3>& poses, Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
boost::shared_ptr<CALIBRATION> sharedCal, boost::shared_ptr<CALIBRATION> sharedCal,
const std::vector<Point2>& measurements, const Point3& initialEstimate) { const Point2Vector& measurements, const Point3& initialEstimate) {
// Create a factor graph and initial values // Create a factor graph and initial values
Values values; Values values;
@ -173,8 +175,8 @@ Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
*/ */
template<class CAMERA> template<class CAMERA>
Point3 triangulateNonlinear( Point3 triangulateNonlinear(
const std::vector<CAMERA>& cameras, const CameraSet<CAMERA>& cameras,
const std::vector<typename CAMERA::Measurement>& measurements, const Point3& initialEstimate) { const typename CAMERA::MeasurementVector& measurements, const Point3& initialEstimate) {
// Create a factor graph and initial values // Create a factor graph and initial values
Values values; Values values;
@ -188,8 +190,8 @@ Point3 triangulateNonlinear(
/// PinholeCamera specific version // TODO: (chris) why does this exist? /// PinholeCamera specific version // TODO: (chris) why does this exist?
template<class CALIBRATION> template<class CALIBRATION>
Point3 triangulateNonlinear( Point3 triangulateNonlinear(
const std::vector<PinholeCamera<CALIBRATION> >& cameras, const CameraSet<PinholeCamera<CALIBRATION> >& cameras,
const std::vector<Point2>& measurements, const Point3& initialEstimate) { const Point2Vector& measurements, const Point3& initialEstimate) {
return triangulateNonlinear<PinholeCamera<CALIBRATION> > // return triangulateNonlinear<PinholeCamera<CALIBRATION> > //
(cameras, measurements, initialEstimate); (cameras, measurements, initialEstimate);
} }
@ -228,7 +230,7 @@ private:
template<class CALIBRATION> template<class CALIBRATION>
Point3 triangulatePoint3(const std::vector<Pose3>& poses, Point3 triangulatePoint3(const std::vector<Pose3>& poses,
boost::shared_ptr<CALIBRATION> sharedCal, boost::shared_ptr<CALIBRATION> sharedCal,
const std::vector<Point2>& measurements, double rank_tol = 1e-9, const Point2Vector& measurements, double rank_tol = 1e-9,
bool optimize = false) { bool optimize = false) {
assert(poses.size() == measurements.size()); assert(poses.size() == measurements.size());
@ -275,8 +277,8 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
*/ */
template<class CAMERA> template<class CAMERA>
Point3 triangulatePoint3( Point3 triangulatePoint3(
const std::vector<CAMERA>& cameras, const CameraSet<CAMERA>& cameras,
const std::vector<Point2>& measurements, double rank_tol = 1e-9, const typename CAMERA::MeasurementVector& measurements, double rank_tol = 1e-9,
bool optimize = false) { bool optimize = false) {
size_t m = cameras.size(); size_t m = cameras.size();
@ -312,8 +314,8 @@ Point3 triangulatePoint3(
/// Pinhole-specific version /// Pinhole-specific version
template<class CALIBRATION> template<class CALIBRATION>
Point3 triangulatePoint3( Point3 triangulatePoint3(
const std::vector<PinholeCamera<CALIBRATION> >& cameras, const CameraSet<PinholeCamera<CALIBRATION> >& cameras,
const std::vector<Point2>& measurements, double rank_tol = 1e-9, const Point2Vector& measurements, double rank_tol = 1e-9,
bool optimize = false) { bool optimize = false) {
return triangulatePoint3<PinholeCamera<CALIBRATION> > // return triangulatePoint3<PinholeCamera<CALIBRATION> > //
(cameras, measurements, rank_tol, optimize); (cameras, measurements, rank_tol, optimize);
@ -453,8 +455,8 @@ private:
/// triangulateSafe: extensive checking of the outcome /// triangulateSafe: extensive checking of the outcome
template<class CAMERA> template<class CAMERA>
TriangulationResult triangulateSafe(const std::vector<CAMERA>& cameras, TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
const std::vector<Point2>& measured, const typename CAMERA::MeasurementVector& measured,
const TriangulationParameters& params) { const TriangulationParameters& params) {
size_t m = cameras.size(); size_t m = cameras.size();

10
gtsam/navigation/CombinedImuFactor.h
View File

@ -83,12 +83,12 @@ public:
// Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
static boost::shared_ptr<Params> MakeSharedD(double g = 9.81) { static boost::shared_ptr<Params> MakeSharedD(double g = 9.81) {
return boost::make_shared<Params>(Vector3(0, 0, g)); return boost::shared_ptr<Params>(new Params(Vector3(0, 0, g)));
} }
// Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
static boost::shared_ptr<Params> MakeSharedU(double g = 9.81) { static boost::shared_ptr<Params> MakeSharedU(double g = 9.81) {
return boost::make_shared<Params>(Vector3(0, 0, -g)); return boost::shared_ptr<Params>(new Params(Vector3(0, 0, -g)));
} }
private: private:
@ -104,6 +104,9 @@ public:
ar& BOOST_SERIALIZATION_NVP(biasOmegaCovariance); ar& BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInt); ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInt);
} }
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
protected: protected:
@ -195,6 +198,9 @@ public:
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType); ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType);
ar& BOOST_SERIALIZATION_NVP(preintMeasCov_); ar& BOOST_SERIALIZATION_NVP(preintMeasCov_);
} }
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
/** /**

3
gtsam/navigation/ImuBias.h
View File

@ -169,6 +169,9 @@ private:
ar & BOOST_SERIALIZATION_NVP(biasGyro_); ar & BOOST_SERIALIZATION_NVP(biasGyro_);
} }
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// @} /// @}
}; // ConstantBias class }; // ConstantBias class

6
gtsam/navigation/PreintegratedRotation.h
View File

@ -58,6 +58,9 @@ struct PreintegratedRotationParams {
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis); ar & BOOST_SERIALIZATION_NVP(omegaCoriolis);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor); ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
} }
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
/** /**
@ -165,6 +168,9 @@ class PreintegratedRotation {
ar& BOOST_SERIALIZATION_NVP(deltaRij_); ar& BOOST_SERIALIZATION_NVP(deltaRij_);
ar& BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); ar& BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
} }
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
template <> template <>

7
gtsam/navigation/PreintegrationParams.h
View File

@ -39,12 +39,12 @@ struct PreintegrationParams: PreintegratedRotationParams {
// Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
static boost::shared_ptr<PreintegrationParams> MakeSharedD(double g = 9.81) { static boost::shared_ptr<PreintegrationParams> MakeSharedD(double g = 9.81) {
return boost::make_shared<PreintegrationParams>(Vector3(0, 0, g)); return boost::shared_ptr<PreintegrationParams>(new PreintegrationParams(Vector3(0, 0, g)));
} }
// Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
static boost::shared_ptr<PreintegrationParams> MakeSharedU(double g = 9.81) { static boost::shared_ptr<PreintegrationParams> MakeSharedU(double g = 9.81) {
return boost::make_shared<PreintegrationParams>(Vector3(0, 0, -g)); return boost::shared_ptr<PreintegrationParams>(new PreintegrationParams(Vector3(0, 0, -g)));
} }
void print(const std::string& s) const; void print(const std::string& s) const;
@ -74,6 +74,9 @@ protected:
ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis); ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis);
ar & BOOST_SERIALIZATION_NVP(n_gravity); ar & BOOST_SERIALIZATION_NVP(n_gravity);
} }
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
} // namespace gtsam } // namespace gtsam

2
gtsam/slam/JacobianFactorQ.h
View File

@ -51,7 +51,7 @@ public:
/// Constructor /// Constructor
JacobianFactorQ(const FastVector<Key>& keys, JacobianFactorQ(const FastVector<Key>& keys,
const std::vector<MatrixZD>& FBlocks, const Matrix& E, const Matrix3& P, const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks, const Matrix& E, const Matrix3& P,
const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : const Vector& b, const SharedDiagonal& model = SharedDiagonal()) :
Base() { Base() {
size_t j = 0, m2 = E.rows(), m = m2 / ZDim; size_t j = 0, m2 = E.rows(), m = m2 / ZDim;

2
gtsam/slam/JacobianFactorQR.h
View File

@ -29,7 +29,7 @@ public:
* Constructor * Constructor
*/ */
JacobianFactorQR(const FastVector<Key>& keys, JacobianFactorQR(const FastVector<Key>& keys,
const std::vector<MatrixZD>& FBlocks, const Matrix& E, const Matrix3& P, const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks, const Matrix& E, const Matrix3& P,
const Vector& b, // const Vector& b, //
const SharedDiagonal& model = SharedDiagonal()) : const SharedDiagonal& model = SharedDiagonal()) :
Base() { Base() {

2
gtsam/slam/JacobianFactorSVD.h
View File

@ -59,7 +59,7 @@ public:
* @Fblocks: * @Fblocks:
*/ */
JacobianFactorSVD(const FastVector<Key>& keys, JacobianFactorSVD(const FastVector<Key>& keys,
const std::vector<MatrixZD>& Fblocks, const Matrix& Enull, const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& Fblocks, const Matrix& Enull,
const Vector& b, // const Vector& b, //
const SharedDiagonal& model = SharedDiagonal()) : const SharedDiagonal& model = SharedDiagonal()) :
Base() { Base() {

6
gtsam/slam/RegularImplicitSchurFactor.h
View File

@ -36,7 +36,7 @@ protected:
typedef Eigen::Matrix<double, ZDim, D> MatrixZD; ///< type of an F block typedef Eigen::Matrix<double, ZDim, D> MatrixZD; ///< type of an F block
typedef Eigen::Matrix<double, D, D> MatrixDD; ///< camera hessian typedef Eigen::Matrix<double, D, D> MatrixDD; ///< camera hessian
const std::vector<MatrixZD> FBlocks_; ///< All ZDim*D F blocks (one for each camera) const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks_; ///< All ZDim*D F blocks (one for each camera)
const Matrix PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (2*2 if degenerate) const Matrix PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (2*2 if degenerate)
const Matrix E_; ///< The 2m*3 E Jacobian with respect to the point const Matrix E_; ///< The 2m*3 E Jacobian with respect to the point
const Vector b_; ///< 2m-dimensional RHS vector const Vector b_; ///< 2m-dimensional RHS vector
@ -49,7 +49,7 @@ public:
/// Construct from blocks of F, E, inv(E'*E), and RHS vector b /// Construct from blocks of F, E, inv(E'*E), and RHS vector b
RegularImplicitSchurFactor(const FastVector<Key>& keys, RegularImplicitSchurFactor(const FastVector<Key>& keys,
const std::vector<MatrixZD>& FBlocks, const Matrix& E, const Matrix& P, const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks, const Matrix& E, const Matrix& P,
const Vector& b) : const Vector& b) :
GaussianFactor(keys), FBlocks_(FBlocks), PointCovariance_(P), E_(E), b_(b) { GaussianFactor(keys), FBlocks_(FBlocks), PointCovariance_(P), E_(E), b_(b) {
} }
@ -58,7 +58,7 @@ public:
virtual ~RegularImplicitSchurFactor() { virtual ~RegularImplicitSchurFactor() {
} }
std::vector<MatrixZD>& FBlocks() const { std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks() const {
return FBlocks_; return FBlocks_;
} }

42
gtsam/slam/SmartFactorBase.h
View File

@ -50,12 +50,14 @@ private:
typedef NonlinearFactor Base; typedef NonlinearFactor Base;
typedef SmartFactorBase<CAMERA> This; typedef SmartFactorBase<CAMERA> This;
typedef typename CAMERA::Measurement Z; typedef typename CAMERA::Measurement Z;
typedef typename CAMERA::MeasurementVector ZVector;
public: public:
static const int Dim = traits<CAMERA>::dimension; ///< Camera dimension static const int Dim = traits<CAMERA>::dimension; ///< Camera dimension
static const int ZDim = traits<Z>::dimension; ///< Measurement dimension static const int ZDim = traits<Z>::dimension; ///< Measurement dimension
typedef Eigen::Matrix<double, ZDim, Dim> MatrixZD; // F blocks (derivatives wrpt camera) typedef Eigen::Matrix<double, ZDim, Dim> MatrixZD; // F blocks (derivatives wrpt camera)
typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks; // vector of F blocks
protected: protected:
/** /**
@ -71,14 +73,14 @@ protected:
* We keep a copy of measurements for I/O and computing the error. * We keep a copy of measurements for I/O and computing the error.
* The order is kept the same as the keys that we use to create the factor. * The order is kept the same as the keys that we use to create the factor.
*/ */
std::vector<Z> measured_; ZVector measured_;
/// @name Pose of the camera in the body frame /// @name Pose of the camera in the body frame
boost::optional<Pose3> body_P_sensor_; ///< Pose of the camera in the body frame boost::optional<Pose3> body_P_sensor_; ///< Pose of the camera in the body frame
/// @} /// @}
// Cache for Fblocks, to avoid a malloc ever time we re-linearize // Cache for Fblocks, to avoid a malloc ever time we re-linearize
mutable std::vector<MatrixZD> Fblocks; mutable FBlocks Fs;
public: public:
@ -97,7 +99,7 @@ public:
SmartFactorBase(const SharedNoiseModel& sharedNoiseModel, SmartFactorBase(const SharedNoiseModel& sharedNoiseModel,
boost::optional<Pose3> body_P_sensor = boost::none, boost::optional<Pose3> body_P_sensor = boost::none,
size_t expectedNumberCameras = 10) size_t expectedNumberCameras = 10)
: body_P_sensor_(body_P_sensor), Fblocks(expectedNumberCameras) { : body_P_sensor_(body_P_sensor), Fs(expectedNumberCameras) {
if (!sharedNoiseModel) if (!sharedNoiseModel)
throw std::runtime_error("SmartFactorBase: sharedNoiseModel is required"); throw std::runtime_error("SmartFactorBase: sharedNoiseModel is required");
@ -129,7 +131,7 @@ public:
/** /**
* Add a bunch of measurements, together with the camera keys * Add a bunch of measurements, together with the camera keys
*/ */
void add(std::vector<Z>& measurements, std::vector<Key>& cameraKeys) { void add(ZVector& measurements, std::vector<Key>& cameraKeys) {
for (size_t i = 0; i < measurements.size(); i++) { for (size_t i = 0; i < measurements.size(); i++) {
this->measured_.push_back(measurements.at(i)); this->measured_.push_back(measurements.at(i));
this->keys_.push_back(cameraKeys.at(i)); this->keys_.push_back(cameraKeys.at(i));
@ -154,7 +156,7 @@ public:
} }
/** return the measurements */ /** return the measurements */
const std::vector<Z>& measured() const { const ZVector& measured() const {
return measured_; return measured_;
} }
@ -258,22 +260,22 @@ public:
* TODO: Kill this obsolete method * TODO: Kill this obsolete method
*/ */
template<class POINT> template<class POINT>
void computeJacobians(std::vector<MatrixZD>& Fblocks, Matrix& E, Vector& b, void computeJacobians(FBlocks& Fs, Matrix& E, Vector& b,
const Cameras& cameras, const POINT& point) const { const Cameras& cameras, const POINT& point) const {
// Project into Camera set and calculate derivatives // Project into Camera set and calculate derivatives
// As in expressionFactor, RHS vector b = - (h(x_bar) - z) = z-h(x_bar) // As in expressionFactor, RHS vector b = - (h(x_bar) - z) = z-h(x_bar)
// Indeed, nonlinear error |h(x_bar+dx)-z| ~ |h(x_bar) + A*dx - z| // Indeed, nonlinear error |h(x_bar+dx)-z| ~ |h(x_bar) + A*dx - z|
// = |A*dx - (z-h(x_bar))| // = |A*dx - (z-h(x_bar))|
b = -unwhitenedError(cameras, point, Fblocks, E); b = -unwhitenedError(cameras, point, Fs, E);
} }
/// SVD version /// SVD version
template<class POINT> template<class POINT>
void computeJacobiansSVD(std::vector<MatrixZD>& Fblocks, Matrix& Enull, void computeJacobiansSVD(FBlocks& Fs, Matrix& Enull,
Vector& b, const Cameras& cameras, const POINT& point) const { Vector& b, const Cameras& cameras, const POINT& point) const {
Matrix E; Matrix E;
computeJacobians(Fblocks, E, b, cameras, point); computeJacobians(Fs, E, b, cameras, point);
static const int N = FixedDimension<POINT>::value; // 2 (Unit3) or 3 (Point3) static const int N = FixedDimension<POINT>::value; // 2 (Unit3) or 3 (Point3)
@ -291,10 +293,10 @@ public:
Matrix E; Matrix E;
Vector b; Vector b;
computeJacobians(Fblocks, E, b, cameras, point); computeJacobians(Fs, E, b, cameras, point);
// build augmented hessian // build augmented hessian
SymmetricBlockMatrix augmentedHessian = Cameras::SchurComplement(Fblocks, E, b); SymmetricBlockMatrix augmentedHessian = Cameras::SchurComplement(Fs, E, b);
return boost::make_shared<RegularHessianFactor<Dim> >(keys_, return boost::make_shared<RegularHessianFactor<Dim> >(keys_,
augmentedHessian); augmentedHessian);
@ -311,12 +313,12 @@ public:
const FastVector<Key> allKeys) const { const FastVector<Key> allKeys) const {
Matrix E; Matrix E;
Vector b; Vector b;
computeJacobians(Fblocks, E, b, cameras, point); computeJacobians(Fs, E, b, cameras, point);
Cameras::UpdateSchurComplement(Fblocks, E, b, allKeys, keys_, augmentedHessian); Cameras::UpdateSchurComplement(Fs, E, b, allKeys, keys_, augmentedHessian);
} }
/// Whiten the Jacobians computed by computeJacobians using noiseModel_ /// Whiten the Jacobians computed by computeJacobians using noiseModel_
void whitenJacobians(std::vector<MatrixZD>& F, Matrix& E, Vector& b) const { void whitenJacobians(FBlocks& F, Matrix& E, Vector& b) const {
noiseModel_->WhitenSystem(E, b); noiseModel_->WhitenSystem(E, b);
// TODO make WhitenInPlace work with any dense matrix type // TODO make WhitenInPlace work with any dense matrix type
for (size_t i = 0; i < F.size(); i++) for (size_t i = 0; i < F.size(); i++)
@ -329,7 +331,7 @@ public:
double lambda = 0.0, bool diagonalDamping = false) const { double lambda = 0.0, bool diagonalDamping = false) const {
Matrix E; Matrix E;
Vector b; Vector b;
std::vector<MatrixZD> F; FBlocks F;
computeJacobians(F, E, b, cameras, point); computeJacobians(F, E, b, cameras, point);
whitenJacobians(F, E, b); whitenJacobians(F, E, b);
Matrix P = Cameras::PointCov(E, lambda, diagonalDamping); Matrix P = Cameras::PointCov(E, lambda, diagonalDamping);
@ -345,7 +347,7 @@ public:
bool diagonalDamping = false) const { bool diagonalDamping = false) const {
Matrix E; Matrix E;
Vector b; Vector b;
std::vector<MatrixZD> F; FBlocks F;
computeJacobians(F, E, b, cameras, point); computeJacobians(F, E, b, cameras, point);
const size_t M = b.size(); const size_t M = b.size();
Matrix P = Cameras::PointCov(E, lambda, diagonalDamping); Matrix P = Cameras::PointCov(E, lambda, diagonalDamping);
@ -360,7 +362,7 @@ public:
boost::shared_ptr<JacobianFactor> createJacobianSVDFactor( boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
const Cameras& cameras, const Point3& point, double lambda = 0.0) const { const Cameras& cameras, const Point3& point, double lambda = 0.0) const {
size_t m = this->keys_.size(); size_t m = this->keys_.size();
std::vector<MatrixZD> F; FBlocks F;
Vector b; Vector b;
const size_t M = ZDim * m; const size_t M = ZDim * m;
Matrix E0(M, M - 3); Matrix E0(M, M - 3);
@ -371,12 +373,12 @@ public:
} }
/// Create BIG block-diagonal matrix F from Fblocks /// Create BIG block-diagonal matrix F from Fblocks
static void FillDiagonalF(const std::vector<MatrixZD>& Fblocks, Matrix& F) { static void FillDiagonalF(const FBlocks& Fs, Matrix& F) {
size_t m = Fblocks.size(); size_t m = Fs.size();
F.resize(ZDim * m, Dim * m); F.resize(ZDim * m, Dim * m);
F.setZero(); F.setZero();
for (size_t i = 0; i < m; ++i) for (size_t i = 0; i < m; ++i)
F.block<ZDim, Dim>(ZDim * i, Dim * i) = Fblocks.at(i); F.block<ZDim, Dim>(ZDim * i, Dim * i) = Fs.at(i);
} }

10
gtsam/slam/SmartProjectionFactor.h
View File

@ -61,7 +61,7 @@ protected:
/// @name Caching triangulation /// @name Caching triangulation
/// @{ /// @{
mutable TriangulationResult result_; ///< result from triangulateSafe mutable TriangulationResult result_; ///< result from triangulateSafe
mutable std::vector<Pose3> cameraPosesTriangulation_; ///< current triangulation poses mutable std::vector<Pose3, Eigen::aligned_allocator<Pose3> > cameraPosesTriangulation_; ///< current triangulation poses
/// @} /// @}
public: public:
@ -203,7 +203,7 @@ public:
} }
// Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols(). // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols().
std::vector<typename Base::MatrixZD> Fblocks; std::vector<typename Base::MatrixZD, Eigen::aligned_allocator<typename Base::MatrixZD> > Fblocks;
Matrix E; Matrix E;
Vector b; Vector b;
computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras); computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
@ -337,7 +337,7 @@ public:
/// Assumes the point has been computed /// Assumes the point has been computed
/// Note E can be 2m*3 or 2m*2, in case point is degenerate /// Note E can be 2m*3 or 2m*2, in case point is degenerate
void computeJacobiansWithTriangulatedPoint( void computeJacobiansWithTriangulatedPoint(
std::vector<typename Base::MatrixZD>& Fblocks, Matrix& E, Vector& b, std::vector<typename Base::MatrixZD, Eigen::aligned_allocator<typename Base::MatrixZD> >& Fblocks, Matrix& E, Vector& b,
const Cameras& cameras) const { const Cameras& cameras) const {
if (!result_) { if (!result_) {
@ -354,7 +354,7 @@ public:
/// Version that takes values, and creates the point /// Version that takes values, and creates the point
bool triangulateAndComputeJacobians( bool triangulateAndComputeJacobians(
std::vector<typename Base::MatrixZD>& Fblocks, Matrix& E, Vector& b, std::vector<typename Base::MatrixZD, Eigen::aligned_allocator<typename Base::MatrixZD> >& Fblocks, Matrix& E, Vector& b,
const Values& values) const { const Values& values) const {
Cameras cameras = this->cameras(values); Cameras cameras = this->cameras(values);
bool nonDegenerate = triangulateForLinearize(cameras); bool nonDegenerate = triangulateForLinearize(cameras);
@ -365,7 +365,7 @@ public:
/// takes values /// takes values
bool triangulateAndComputeJacobiansSVD( bool triangulateAndComputeJacobiansSVD(
std::vector<typename Base::MatrixZD>& Fblocks, Matrix& Enull, Vector& b, std::vector<typename Base::MatrixZD, Eigen::aligned_allocator<typename Base::MatrixZD> >& Fblocks, Matrix& Enull, Vector& b,
const Values& values) const { const Values& values) const {
Cameras cameras = this->cameras(values); Cameras cameras = this->cameras(values);
bool nonDegenerate = triangulateForLinearize(cameras); bool nonDegenerate = triangulateForLinearize(cameras);

2
gtsam/slam/tests/smartFactorScenarios.h
View File

@ -132,7 +132,7 @@ CAMERA perturbCameraPose(const CAMERA& camera) {
template<class CAMERA> template<class CAMERA>
void projectToMultipleCameras(const CAMERA& cam1, const CAMERA& cam2, void projectToMultipleCameras(const CAMERA& cam1, const CAMERA& cam2,
const CAMERA& cam3, Point3 landmark, vector<Point2>& measurements_cam) { const CAMERA& cam3, Point3 landmark, typename CAMERA::MeasurementVector& measurements_cam) {
Point2 cam1_uv1 = cam1.project(landmark); Point2 cam1_uv1 = cam1.project(landmark);
Point2 cam2_uv1 = cam2.project(landmark); Point2 cam2_uv1 = cam2.project(landmark);
Point2 cam3_uv1 = cam3.project(landmark); Point2 cam3_uv1 = cam3.project(landmark);

2
gtsam/slam/tests/testRegularImplicitSchurFactor.cpp
View File

@ -41,7 +41,7 @@ using namespace gtsam;
const Matrix26 F0 = Matrix26::Ones(); const Matrix26 F0 = Matrix26::Ones();
const Matrix26 F1 = 2 * Matrix26::Ones(); const Matrix26 F1 = 2 * Matrix26::Ones();
const Matrix26 F3 = 3 * Matrix26::Ones(); const Matrix26 F3 = 3 * Matrix26::Ones();
const vector<Matrix26> FBlocks = list_of<Matrix26>(F0)(F1)(F3); const vector<Matrix26, Eigen::aligned_allocator<Matrix26> > FBlocks = list_of<Matrix26>(F0)(F1)(F3);
const FastVector<Key> keys = list_of<Key>(0)(1)(3); const FastVector<Key> keys = list_of<Key>(0)(1)(3);
// RHS and sigmas // RHS and sigmas
const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.).finished(); const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.).finished();

16
gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
View File

@ -166,7 +166,7 @@ TEST( SmartProjectionCameraFactor, noisy ) {
EXPECT(assert_equal(expected, actual, 1)); EXPECT(assert_equal(expected, actual, 1));
SmartFactor::shared_ptr factor2(new SmartFactor(unit2)); SmartFactor::shared_ptr factor2(new SmartFactor(unit2));
vector<Point2> measurements; Point2Vector measurements;
measurements.push_back(level_uv); measurements.push_back(level_uv);
measurements.push_back(level_uv_right); measurements.push_back(level_uv_right);
@ -186,7 +186,7 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) {
using namespace vanilla; using namespace vanilla;
// Project three landmarks into three cameras // Project three landmarks into three cameras
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
@ -288,7 +288,7 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
using namespace vanilla; using namespace vanilla;
// Project three landmarks into three cameras // Project three landmarks into three cameras
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
@ -360,7 +360,7 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) {
using namespace vanilla; using namespace vanilla;
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3, Point2Vector measurements_cam1, measurements_cam2, measurements_cam3,
measurements_cam4, measurements_cam5; measurements_cam4, measurements_cam5;
// 1. Project three landmarks into three cameras and triangulate // 1. Project three landmarks into three cameras and triangulate
@ -440,7 +440,7 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler ) {
using namespace bundler; using namespace bundler;
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3, Point2Vector measurements_cam1, measurements_cam2, measurements_cam3,
measurements_cam4, measurements_cam5; measurements_cam4, measurements_cam5;
// 1. Project three landmarks into three cameras and triangulate // 1. Project three landmarks into three cameras and triangulate
@ -516,7 +516,7 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) {
using namespace bundler; using namespace bundler;
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3, Point2Vector measurements_cam1, measurements_cam2, measurements_cam3,
measurements_cam4, measurements_cam5; measurements_cam4, measurements_cam5;
// 1. Project three landmarks into three cameras and triangulate // 1. Project three landmarks into three cameras and triangulate
@ -768,8 +768,8 @@ TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) {
Point3 point(0,0,0); Point3 point(0,0,0);
if (factor1->point()) if (factor1->point())
point = *(factor1->point()); point = *(factor1->point());
vector<Matrix29> Fblocks; SmartFactor::FBlocks Fs;
factor1->computeJacobians(Fblocks, expectedE, expectedb, cameras, point); factor1->computeJacobians(Fs, expectedE, expectedb, cameras, point);
NonlinearFactorGraph generalGraph; NonlinearFactorGraph generalGraph;
SFMFactor sfm1(level_uv, unit2, c1, l1); SFMFactor sfm1(level_uv, unit2, c1, l1);

50
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -155,8 +155,8 @@ TEST( SmartProjectionPoseFactor, noiseless ) {
// Calculate using computeJacobians // Calculate using computeJacobians
Vector b; Vector b;
vector<SmartFactor::MatrixZD> Fblocks; SmartFactor::FBlocks Fs;
factor.computeJacobians(Fblocks, E, b, cameras, *point); factor.computeJacobians(Fs, E, b, cameras, *point);
double actualError3 = b.squaredNorm(); double actualError3 = b.squaredNorm();
EXPECT(assert_equal(expectedE, E, 1e-7)); EXPECT(assert_equal(expectedE, E, 1e-7));
EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-8); EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-8);
@ -185,7 +185,7 @@ TEST( SmartProjectionPoseFactor, noisy ) {
double actualError1 = factor->error(values); double actualError1 = factor->error(values);
SmartFactor::shared_ptr factor2(new SmartFactor(model, sharedK)); SmartFactor::shared_ptr factor2(new SmartFactor(model, sharedK));
vector<Point2> measurements; Point2Vector measurements;
measurements.push_back(level_uv); measurements.push_back(level_uv);
measurements.push_back(level_uv_right); measurements.push_back(level_uv_right);
@ -228,7 +228,7 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){
Point3 landmark2(5, -0.5, 1.2); Point3 landmark2(5, -0.5, 1.2);
Point3 landmark3(5, 0, 3.0); Point3 landmark3(5, 0, 3.0);
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
// Project three landmarks into three cameras // Project three landmarks into three cameras
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@ -292,7 +292,7 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){
TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
using namespace vanillaPose2; using namespace vanillaPose2;
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
// Project three landmarks into three cameras // Project three landmarks into three cameras
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@ -363,7 +363,7 @@ TEST( SmartProjectionPoseFactor, Factors ) {
// one landmarks 1m in front of camera // one landmarks 1m in front of camera
Point3 landmark1(0, 0, 10); Point3 landmark1(0, 0, 10);
vector<Point2> measurements_cam1; Point2Vector measurements_cam1;
// Project 2 landmarks into 2 cameras // Project 2 landmarks into 2 cameras
measurements_cam1.push_back(cam1.project(landmark1)); measurements_cam1.push_back(cam1.project(landmark1));
@ -454,7 +454,7 @@ TEST( SmartProjectionPoseFactor, Factors ) {
E(2, 0) = 10; E(2, 0) = 10;
E(2, 2) = 1; E(2, 2) = 1;
E(3, 1) = 10; E(3, 1) = 10;
vector<Matrix26> Fblocks = list_of<Matrix>(F1)(F2); SmartFactor::FBlocks Fs = list_of<Matrix>(F1)(F2);
Vector b(4); Vector b(4);
b.setZero(); b.setZero();
@ -466,7 +466,7 @@ TEST( SmartProjectionPoseFactor, Factors ) {
// createJacobianQFactor // createJacobianQFactor
SharedIsotropic n = noiseModel::Isotropic::Sigma(4, sigma); SharedIsotropic n = noiseModel::Isotropic::Sigma(4, sigma);
Matrix3 P = (E.transpose() * E).inverse(); Matrix3 P = (E.transpose() * E).inverse();
JacobianFactorQ<6, 2> expectedQ(keys, Fblocks, E, P, b, n); JacobianFactorQ<6, 2> expectedQ(keys, Fs, E, P, b, n);
EXPECT(assert_equal(expectedInformation, expectedQ.information(), 1e-8)); EXPECT(assert_equal(expectedInformation, expectedQ.information(), 1e-8));
boost::shared_ptr<JacobianFactorQ<6, 2> > actualQ = boost::shared_ptr<JacobianFactorQ<6, 2> > actualQ =
@ -480,11 +480,11 @@ TEST( SmartProjectionPoseFactor, Factors ) {
// Whiten for RegularImplicitSchurFactor (does not have noise model) // Whiten for RegularImplicitSchurFactor (does not have noise model)
model->WhitenSystem(E, b); model->WhitenSystem(E, b);
Matrix3 whiteP = (E.transpose() * E).inverse(); Matrix3 whiteP = (E.transpose() * E).inverse();
Fblocks[0] = model->Whiten(Fblocks[0]); Fs[0] = model->Whiten(Fs[0]);
Fblocks[1] = model->Whiten(Fblocks[1]); Fs[1] = model->Whiten(Fs[1]);
// createRegularImplicitSchurFactor // createRegularImplicitSchurFactor
RegularImplicitSchurFactor<Camera> expected(keys, Fblocks, E, whiteP, b); RegularImplicitSchurFactor<Camera> expected(keys, Fs, E, whiteP, b);
boost::shared_ptr<RegularImplicitSchurFactor<Camera> > actual = boost::shared_ptr<RegularImplicitSchurFactor<Camera> > actual =
smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0); smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0);
@ -525,7 +525,7 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
// Project three landmarks into three cameras // Project three landmarks into three cameras
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@ -582,7 +582,7 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
// Project three landmarks into three cameras // Project three landmarks into three cameras
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@ -643,7 +643,7 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) {
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
// Project three landmarks into three cameras // Project three landmarks into three cameras
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@ -709,7 +709,7 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) {
// add fourth landmark // add fourth landmark
Point3 landmark4(5, -0.5, 1); Point3 landmark4(5, -0.5, 1);
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3, Point2Vector measurements_cam1, measurements_cam2, measurements_cam3,
measurements_cam4; measurements_cam4;
// Project 4 landmarks into three cameras // Project 4 landmarks into three cameras
@ -772,7 +772,7 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) {
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
// Project three landmarks into three cameras // Project three landmarks into three cameras
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@ -883,7 +883,7 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
Camera cam2(pose2, sharedK); Camera cam2(pose2, sharedK);
Camera cam3(pose3, sharedK); Camera cam3(pose3, sharedK);
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
// Project three landmarks into three cameras // Project three landmarks into three cameras
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@ -966,7 +966,7 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
Camera cam2(pose2, sharedK2); Camera cam2(pose2, sharedK2);
Camera cam3(pose3, sharedK2); Camera cam3(pose3, sharedK2);
vector<Point2> measurements_cam1, measurements_cam2; Point2Vector measurements_cam1, measurements_cam2;
// Project three landmarks into three cameras // Project three landmarks into three cameras
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@ -1026,7 +1026,7 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
Camera cam2(pose2, sharedK); Camera cam2(pose2, sharedK);
Camera cam3(pose3, sharedK); Camera cam3(pose3, sharedK);
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
// Project three landmarks into three cameras // Project three landmarks into three cameras
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@ -1106,7 +1106,7 @@ TEST( SmartProjectionPoseFactor, Hessian ) {
// Project three landmarks into 2 cameras // Project three landmarks into 2 cameras
Point2 cam1_uv1 = cam1.project(landmark1); Point2 cam1_uv1 = cam1.project(landmark1);
Point2 cam2_uv1 = cam2.project(landmark1); Point2 cam2_uv1 = cam2.project(landmark1);
vector<Point2> measurements_cam1; Point2Vector measurements_cam1;
measurements_cam1.push_back(cam1_uv1); measurements_cam1.push_back(cam1_uv1);
measurements_cam1.push_back(cam2_uv1); measurements_cam1.push_back(cam2_uv1);
@ -1138,7 +1138,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) {
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@ -1195,7 +1195,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) {
Camera cam2(level_pose, sharedK2); Camera cam2(level_pose, sharedK2);
Camera cam3(level_pose, sharedK2); Camera cam3(level_pose, sharedK2);
vector<Point2> measurements_cam1; Point2Vector measurements_cam1;
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedK2)); SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedK2));
@ -1253,7 +1253,7 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) {
// three landmarks ~5 meters in front of camera // three landmarks ~5 meters in front of camera
Point3 landmark3(3, 0, 3.0); Point3 landmark3(3, 0, 3.0);
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
// Project three landmarks into three cameras // Project three landmarks into three cameras
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@ -1324,7 +1324,7 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
// landmark3 at 3 meters now // landmark3 at 3 meters now
Point3 landmark3(3, 0, 3.0); Point3 landmark3(3, 0, 3.0);
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
// Project three landmarks into three cameras // Project three landmarks into three cameras
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@ -1422,7 +1422,7 @@ TEST(SmartProjectionPoseFactor, serialize2) {
// insert some measurments // insert some measurments
vector<Key> key_view; vector<Key> key_view;
vector<Point2> meas_view; Point2Vector meas_view;
key_view.push_back(Symbol('x', 1)); key_view.push_back(Symbol('x', 1));
meas_view.push_back(Point2(10, 10)); meas_view.push_back(Point2(10, 10));
factor.add(meas_view, key_view); factor.add(meas_view, key_view);

25
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -78,7 +78,7 @@ public:
/// Vector of monocular cameras (stereo treated as 2 monocular) /// Vector of monocular cameras (stereo treated as 2 monocular)
typedef PinholeCamera<Cal3_S2> MonoCamera; typedef PinholeCamera<Cal3_S2> MonoCamera;
typedef CameraSet<MonoCamera> MonoCameras; typedef CameraSet<MonoCamera> MonoCameras;
typedef std::vector<Point2> MonoMeasurements; typedef MonoCamera::MeasurementVector MonoMeasurements;
/** /**
* Constructor * Constructor
@ -226,17 +226,17 @@ public:
} }
// Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols(). // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols().
std::vector<Base::MatrixZD> Fblocks; Base::FBlocks Fs;
Matrix F, E; Matrix F, E;
Vector b; Vector b;
computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras); computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras);
// Whiten using noise model // Whiten using noise model
Base::whitenJacobians(Fblocks, E, b); Base::whitenJacobians(Fs, E, b);
// build augmented hessian // build augmented hessian
SymmetricBlockMatrix augmentedHessian = // SymmetricBlockMatrix augmentedHessian = //
Cameras::SchurComplement(Fblocks, E, b, lambda, diagonalDamping); Cameras::SchurComplement(Fs, E, b, lambda, diagonalDamping);
return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_, return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
augmentedHessian); augmentedHessian);
@ -360,7 +360,8 @@ public:
/// Assumes the point has been computed /// Assumes the point has been computed
/// Note E can be 2m*3 or 2m*2, in case point is degenerate /// Note E can be 2m*3 or 2m*2, in case point is degenerate
void computeJacobiansWithTriangulatedPoint( void computeJacobiansWithTriangulatedPoint(
std::vector<Base::MatrixZD>& Fblocks, Matrix& E, Vector& b, FBlocks& Fs,
Matrix& E, Vector& b,
const Cameras& cameras) const { const Cameras& cameras) const {
if (!result_) { if (!result_) {
@ -370,32 +371,32 @@ public:
// Unit3 backProjected; /* = cameras[0].backprojectPointAtInfinity( // Unit3 backProjected; /* = cameras[0].backprojectPointAtInfinity(
// this->measured_.at(0)); */ // this->measured_.at(0)); */
// //
// Base::computeJacobians(Fblocks, E, b, cameras, backProjected); // Base::computeJacobians(Fs, E, b, cameras, backProjected);
} else { } else {
// valid result: just return Base version // valid result: just return Base version
Base::computeJacobians(Fblocks, E, b, cameras, *result_); Base::computeJacobians(Fs, E, b, cameras, *result_);
} }
} }
/// Version that takes values, and creates the point /// Version that takes values, and creates the point
bool triangulateAndComputeJacobians( bool triangulateAndComputeJacobians(
std::vector<Base::MatrixZD>& Fblocks, Matrix& E, Vector& b, FBlocks& Fs, Matrix& E, Vector& b,
const Values& values) const { const Values& values) const {
Cameras cameras = this->cameras(values); Cameras cameras = this->cameras(values);
bool nonDegenerate = triangulateForLinearize(cameras); bool nonDegenerate = triangulateForLinearize(cameras);
if (nonDegenerate) if (nonDegenerate)
computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras); computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras);
return nonDegenerate; return nonDegenerate;
} }
/// takes values /// takes values
bool triangulateAndComputeJacobiansSVD( bool triangulateAndComputeJacobiansSVD(
std::vector<Base::MatrixZD>& Fblocks, Matrix& Enull, Vector& b, FBlocks& Fs, Matrix& Enull, Vector& b,
const Values& values) const { const Values& values) const {
Cameras cameras = this->cameras(values); Cameras cameras = this->cameras(values);
bool nonDegenerate = triangulateForLinearize(cameras); bool nonDegenerate = triangulateForLinearize(cameras);
if (nonDegenerate) if (nonDegenerate)
Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, *result_); Base::computeJacobiansSVD(Fs, Enull, b, cameras, *result_);
return nonDegenerate; return nonDegenerate;
} }

2
gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
View File

@ -540,7 +540,7 @@ TEST( SmartStereoProjectionPoseFactor, body_P_sensor_monocular ){
Point3 landmark2(5, -0.5, 1.2); Point3 landmark2(5, -0.5, 1.2);
Point3 landmark3(5, 0, 3.0); Point3 landmark3(5, 0, 3.0);
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
// Project three landmarks into three cameras // Project three landmarks into three cameras
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);