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.
*/
typedef Point2 Measurement;
typedef Point2Vector MeasurementVector;
private:

18
gtsam/geometry/CameraSet.h
View File

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

1
gtsam/geometry/PinholeCamera.h
View File

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

2
gtsam/geometry/PinholeSet.h
View File

@ -58,7 +58,7 @@ public:
/// triangulateSafe
TriangulationResult triangulateSafe(
const std::vector<Point2>& measured,
const typename CAMERA::MeasurementVector& measured,
const TriangulationParameters& params) const {
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);
// For MATLAB wrapper
typedef std::vector<Point2> Point2Vector;
typedef std::vector<Point2, Eigen::aligned_allocator<Point2> > Point2Vector;
/// multiply with scalar
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

3
gtsam/geometry/Rot3.h
View File

@ -508,6 +508,9 @@ namespace gtsam {
#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.
*/
typedef StereoPoint2 Measurement;
typedef StereoPoint2Vector MeasurementVector;
private:
Pose3 leftCamPose_;

2
gtsam/geometry/StereoPoint2.h
View File

@ -159,6 +159,8 @@ private:
};
typedef std::vector<StereoPoint2> StereoPoint2Vector;
template<>
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) {
typedef PinholeCamera<Cal3Bundler> Camera;
typedef CameraSet<Camera> Set;
typedef vector<Point2> ZZ;
typedef Point2Vector ZZ;
Set set;
Camera camera;
set.push_back(camera);
@ -135,8 +135,8 @@ TEST(CameraSet, Pinhole) {
/* ************************************************************************* */
#include <gtsam/geometry/StereoCamera.h>
TEST(CameraSet, Stereo) {
typedef vector<StereoPoint2> ZZ;
CameraSet<StereoCamera> set;
typedef StereoCamera::MeasurementVector ZZ;
StereoCamera 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>
TEST(PinholeSet, Stereo) {
typedef vector<Point2> ZZ;
typedef Point2Vector ZZ;
PinholeSet<CalibratedCamera> set;
CalibratedCamera camera;
set.push_back(camera);
@ -72,7 +72,7 @@ TEST(PinholeSet, Stereo) {
#include <gtsam/geometry/Cal3Bundler.h>
TEST(PinholeSet, Pinhole) {
typedef PinholeCamera<Cal3Bundler> Camera;
typedef vector<Point2> ZZ;
typedef Point2Vector ZZ;
PinholeSet<Camera> set;
Camera 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_;};
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 Triangle& t1 = trianglePair.first, t2 = trianglePair.second;
vector<Point2Pair> correspondences;
@ -755,7 +755,7 @@ namespace {
TEST(Pose2, align_4) {
using namespace align_3;
vector<Point2> ps,qs;
Point2Vector ps,qs;
ps += p1, p2, p3;
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) {
vector<Pose3> poses;
vector<Point2> measurements;
Point2Vector measurements;
poses += pose1, pose2;
measurements += z1, z2;
@ -108,7 +108,7 @@ TEST( triangulation, twoPosesBundler) {
Point2 z2 = camera2.project(landmark);
vector<Pose3> poses;
vector<Point2> measurements;
Point2Vector measurements;
poses += pose1, pose2;
measurements += z1, z2;
@ -132,7 +132,7 @@ TEST( triangulation, twoPosesBundler) {
//******************************************************************************
TEST( triangulation, fourPoses) {
vector<Pose3> poses;
vector<Point2> measurements;
Point2Vector measurements;
poses += pose1, pose2;
measurements += z1, z2;
@ -195,8 +195,8 @@ TEST( triangulation, fourPoses_distinct_Ks) {
Point2 z1 = camera1.project(landmark);
Point2 z2 = camera2.project(landmark);
vector<SimpleCamera> cameras;
vector<Point2> measurements;
CameraSet<SimpleCamera> cameras;
Point2Vector measurements;
cameras += camera1, camera2;
measurements += z1, z2;
@ -260,8 +260,8 @@ TEST( triangulation, outliersAndFarLandmarks) {
Point2 z1 = camera1.project(landmark);
Point2 z2 = camera2.project(landmark);
vector<SimpleCamera> cameras;
vector<Point2> measurements;
CameraSet<SimpleCamera> cameras;
Point2Vector measurements;
cameras += camera1, camera2;
measurements += z1, z2;
@ -308,7 +308,7 @@ TEST( triangulation, twoIdenticalPoses) {
Point2 z1 = camera1.project(landmark);
vector<Pose3> poses;
vector<Point2> measurements;
Point2Vector measurements;
poses += pose1, pose1;
measurements += z1, z1;
@ -323,7 +323,7 @@ TEST( triangulation, onePose) {
// because there's only one camera observation
vector<Pose3> poses;
vector<Point2> measurements;
Point2Vector measurements;
poses += Pose3();
measurements += Point2(0,0);
@ -354,7 +354,7 @@ TEST( triangulation, StereotriangulateNonlinear ) {
cameras.push_back(StereoCamera(Pose3(m1), stereoK));
cameras.push_back(StereoCamera(Pose3(m2), stereoK));
vector<StereoPoint2> measurements;
StereoPoint2Vector measurements;
measurements += StereoPoint2(226.936, 175.212, 424.469);
measurements += StereoPoint2(339.571, 285.547, 669.973);

4
gtsam/geometry/triangulation.cpp
View File

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

40
gtsam/geometry/triangulation.h
View File

@ -19,6 +19,7 @@
#pragma once
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/CameraSet.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/slam/TriangulationFactor.h>
#include <gtsam/slam/PriorFactor.h>
@ -53,7 +54,7 @@ public:
*/
GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
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
@ -64,7 +65,8 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
*/
GTSAM_EXPORT Point3 triangulateDLT(
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
@ -78,7 +80,7 @@ GTSAM_EXPORT Point3 triangulateDLT(
template<class CALIBRATION>
std::pair<NonlinearFactorGraph, Values> triangulationGraph(
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) {
Values values;
values.insert(landmarkKey, initialEstimate); // Initial landmark value
@ -106,8 +108,8 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
*/
template<class CAMERA>
std::pair<NonlinearFactorGraph, Values> triangulationGraph(
const std::vector<CAMERA>& cameras,
const std::vector<typename CAMERA::Measurement>& measurements, Key landmarkKey,
const CameraSet<CAMERA>& cameras,
const typename CAMERA::MeasurementVector& measurements, Key landmarkKey,
const Point3& initialEstimate) {
Values values;
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?
template<class CALIBRATION>
std::pair<NonlinearFactorGraph, Values> triangulationGraph(
const std::vector<PinholeCamera<CALIBRATION> >& cameras,
const std::vector<Point2>& measurements, Key landmarkKey,
const CameraSet<PinholeCamera<CALIBRATION> >& cameras,
const Point2Vector& measurements, Key landmarkKey,
const Point3& initialEstimate) {
return triangulationGraph<PinholeCamera<CALIBRATION> > //
(cameras, measurements, landmarkKey, initialEstimate);
@ -153,7 +155,7 @@ GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph,
template<class CALIBRATION>
Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
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
Values values;
@ -173,8 +175,8 @@ Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
*/
template<class CAMERA>
Point3 triangulateNonlinear(
const std::vector<CAMERA>& cameras,
const std::vector<typename CAMERA::Measurement>& measurements, const Point3& initialEstimate) {
const CameraSet<CAMERA>& cameras,
const typename CAMERA::MeasurementVector& measurements, const Point3& initialEstimate) {
// Create a factor graph and initial values
Values values;
@ -188,8 +190,8 @@ Point3 triangulateNonlinear(
/// PinholeCamera specific version // TODO: (chris) why does this exist?
template<class CALIBRATION>
Point3 triangulateNonlinear(
const std::vector<PinholeCamera<CALIBRATION> >& cameras,
const std::vector<Point2>& measurements, const Point3& initialEstimate) {
const CameraSet<PinholeCamera<CALIBRATION> >& cameras,
const Point2Vector& measurements, const Point3& initialEstimate) {
return triangulateNonlinear<PinholeCamera<CALIBRATION> > //
(cameras, measurements, initialEstimate);
}
@ -228,7 +230,7 @@ private:
template<class CALIBRATION>
Point3 triangulatePoint3(const std::vector<Pose3>& poses,
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) {
assert(poses.size() == measurements.size());
@ -275,8 +277,8 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
*/
template<class CAMERA>
Point3 triangulatePoint3(
const std::vector<CAMERA>& cameras,
const std::vector<Point2>& measurements, double rank_tol = 1e-9,
const CameraSet<CAMERA>& cameras,
const typename CAMERA::MeasurementVector& measurements, double rank_tol = 1e-9,
bool optimize = false) {
size_t m = cameras.size();
@ -312,8 +314,8 @@ Point3 triangulatePoint3(
/// Pinhole-specific version
template<class CALIBRATION>
Point3 triangulatePoint3(
const std::vector<PinholeCamera<CALIBRATION> >& cameras,
const std::vector<Point2>& measurements, double rank_tol = 1e-9,
const CameraSet<PinholeCamera<CALIBRATION> >& cameras,
const Point2Vector& measurements, double rank_tol = 1e-9,
bool optimize = false) {
return triangulatePoint3<PinholeCamera<CALIBRATION> > //
(cameras, measurements, rank_tol, optimize);
@ -453,8 +455,8 @@ private:
/// triangulateSafe: extensive checking of the outcome
template<class CAMERA>
TriangulationResult triangulateSafe(const std::vector<CAMERA>& cameras,
const std::vector<Point2>& measured,
TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
const typename CAMERA::MeasurementVector& measured,
const TriangulationParameters& params) {
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
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
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:
@ -104,6 +104,9 @@ public:
ar& BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInt);
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
protected:
@ -195,6 +198,9 @@ public:
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType);
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_);
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// @}
}; // ConstantBias class

6
gtsam/navigation/PreintegratedRotation.h
View File

@ -58,6 +58,9 @@ struct PreintegratedRotationParams {
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis);
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(delRdelBiasOmega_);
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
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
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
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;
@ -74,6 +74,9 @@ protected:
ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis);
ar & BOOST_SERIALIZATION_NVP(n_gravity);
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace gtsam

2
gtsam/slam/JacobianFactorQ.h
View File

@ -51,7 +51,7 @@ public:
/// Constructor
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()) :
Base() {
size_t j = 0, m2 = E.rows(), m = m2 / ZDim;

2
gtsam/slam/JacobianFactorQR.h
View File

@ -29,7 +29,7 @@ public:
* Constructor
*/
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 SharedDiagonal& model = SharedDiagonal()) :
Base() {

2
gtsam/slam/JacobianFactorSVD.h
View File

@ -59,7 +59,7 @@ public:
* @Fblocks:
*/
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 SharedDiagonal& model = SharedDiagonal()) :
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, 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 E_; ///< The 2m*3 E Jacobian with respect to the point
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
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) :
GaussianFactor(keys), FBlocks_(FBlocks), PointCovariance_(P), E_(E), b_(b) {
}
@ -58,7 +58,7 @@ public:
virtual ~RegularImplicitSchurFactor() {
}
std::vector<MatrixZD>& FBlocks() const {
std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks() const {
return FBlocks_;
}

42
gtsam/slam/SmartFactorBase.h
View File

@ -50,12 +50,14 @@ private:
typedef NonlinearFactor Base;
typedef SmartFactorBase<CAMERA> This;
typedef typename CAMERA::Measurement Z;
typedef typename CAMERA::MeasurementVector ZVector;
public:
static const int Dim = traits<CAMERA>::dimension; ///< Camera dimension
static const int ZDim = traits<Z>::dimension; ///< Measurement dimension
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:
/**
@ -71,14 +73,14 @@ protected:
* 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.
*/
std::vector<Z> measured_;
ZVector measured_;
/// @name 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
mutable std::vector<MatrixZD> Fblocks;
mutable FBlocks Fs;
public:
@ -97,7 +99,7 @@ public:
SmartFactorBase(const SharedNoiseModel& sharedNoiseModel,
boost::optional<Pose3> body_P_sensor = boost::none,
size_t expectedNumberCameras = 10)
: body_P_sensor_(body_P_sensor), Fblocks(expectedNumberCameras) {
: body_P_sensor_(body_P_sensor), Fs(expectedNumberCameras) {
if (!sharedNoiseModel)
throw std::runtime_error("SmartFactorBase: sharedNoiseModel is required");
@ -129,7 +131,7 @@ public:
/**
* 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++) {
this->measured_.push_back(measurements.at(i));
this->keys_.push_back(cameraKeys.at(i));
@ -154,7 +156,7 @@ public:
}
/** return the measurements */
const std::vector<Z>& measured() const {
const ZVector& measured() const {
return measured_;
}
@ -258,22 +260,22 @@ public:
* TODO: Kill this obsolete method
*/
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 {
// Project into Camera set and calculate derivatives
// 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|
// = |A*dx - (z-h(x_bar))|
b = -unwhitenedError(cameras, point, Fblocks, E);
b = -unwhitenedError(cameras, point, Fs, E);
}
/// SVD version
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 {
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)
@ -291,10 +293,10 @@ public:
Matrix E;
Vector b;
computeJacobians(Fblocks, E, b, cameras, point);
computeJacobians(Fs, E, b, cameras, point);
// build augmented hessian
SymmetricBlockMatrix augmentedHessian = Cameras::SchurComplement(Fblocks, E, b);
SymmetricBlockMatrix augmentedHessian = Cameras::SchurComplement(Fs, E, b);
return boost::make_shared<RegularHessianFactor<Dim> >(keys_,
augmentedHessian);
@ -311,12 +313,12 @@ public:
const FastVector<Key> allKeys) const {
Matrix E;
Vector b;
computeJacobians(Fblocks, E, b, cameras, point);
Cameras::UpdateSchurComplement(Fblocks, E, b, allKeys, keys_, augmentedHessian);
computeJacobians(Fs, E, b, cameras, point);
Cameras::UpdateSchurComplement(Fs, E, b, allKeys, keys_, augmentedHessian);
}
/// 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);
// TODO make WhitenInPlace work with any dense matrix type
for (size_t i = 0; i < F.size(); i++)
@ -329,7 +331,7 @@ public:
double lambda = 0.0, bool diagonalDamping = false) const {
Matrix E;
Vector b;
std::vector<MatrixZD> F;
FBlocks F;
computeJacobians(F, E, b, cameras, point);
whitenJacobians(F, E, b);
Matrix P = Cameras::PointCov(E, lambda, diagonalDamping);
@ -345,7 +347,7 @@ public:
bool diagonalDamping = false) const {
Matrix E;
Vector b;
std::vector<MatrixZD> F;
FBlocks F;
computeJacobians(F, E, b, cameras, point);
const size_t M = b.size();
Matrix P = Cameras::PointCov(E, lambda, diagonalDamping);
@ -360,7 +362,7 @@ public:
boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
const Cameras& cameras, const Point3& point, double lambda = 0.0) const {
size_t m = this->keys_.size();
std::vector<MatrixZD> F;
FBlocks F;
Vector b;
const size_t M = ZDim * m;
Matrix E0(M, M - 3);
@ -371,12 +373,12 @@ public:
}
/// Create BIG block-diagonal matrix F from Fblocks
static void FillDiagonalF(const std::vector<MatrixZD>& Fblocks, Matrix& F) {
size_t m = Fblocks.size();
static void FillDiagonalF(const FBlocks& Fs, Matrix& F) {
size_t m = Fs.size();
F.resize(ZDim * m, Dim * m);
F.setZero();
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
/// @{
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:
@ -203,7 +203,7 @@ public:
}
// 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;
Vector b;
computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
@ -337,7 +337,7 @@ public:
/// Assumes the point has been computed
/// Note E can be 2m*3 or 2m*2, in case point is degenerate
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 {
if (!result_) {
@ -354,7 +354,7 @@ public:
/// Version that takes values, and creates the point
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 {
Cameras cameras = this->cameras(values);
bool nonDegenerate = triangulateForLinearize(cameras);
@ -365,7 +365,7 @@ public:
/// takes values
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 {
Cameras cameras = this->cameras(values);
bool nonDegenerate = triangulateForLinearize(cameras);

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

@ -132,7 +132,7 @@ CAMERA perturbCameraPose(const CAMERA& camera) {
template<class CAMERA>
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 cam2_uv1 = cam2.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 F1 = 2 * 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);
// RHS and sigmas
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));
SmartFactor::shared_ptr factor2(new SmartFactor(unit2));
vector<Point2> measurements;
Point2Vector measurements;
measurements.push_back(level_uv);
measurements.push_back(level_uv_right);
@ -186,7 +186,7 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) {
using namespace vanilla;
// 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, landmark2, measurements_cam2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
@ -288,7 +288,7 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
using namespace vanilla;
// 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, landmark2, measurements_cam2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
@ -360,7 +360,7 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) {
using namespace vanilla;
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3,
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3,
measurements_cam4, measurements_cam5;
// 1. Project three landmarks into three cameras and triangulate
@ -440,7 +440,7 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler ) {
using namespace bundler;
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3,
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3,
measurements_cam4, measurements_cam5;
// 1. Project three landmarks into three cameras and triangulate
@ -516,7 +516,7 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) {
using namespace bundler;
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3,
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3,
measurements_cam4, measurements_cam5;
// 1. Project three landmarks into three cameras and triangulate
@ -768,8 +768,8 @@ TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) {
Point3 point(0,0,0);
if (factor1->point())
point = *(factor1->point());
vector<Matrix29> Fblocks;
factor1->computeJacobians(Fblocks, expectedE, expectedb, cameras, point);
SmartFactor::FBlocks Fs;
factor1->computeJacobians(Fs, expectedE, expectedb, cameras, point);
NonlinearFactorGraph generalGraph;
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
Vector b;
vector<SmartFactor::MatrixZD> Fblocks;
factor.computeJacobians(Fblocks, E, b, cameras, *point);
SmartFactor::FBlocks Fs;
factor.computeJacobians(Fs, E, b, cameras, *point);
double actualError3 = b.squaredNorm();
EXPECT(assert_equal(expectedE, E, 1e-7));
EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-8);
@ -185,7 +185,7 @@ TEST( SmartProjectionPoseFactor, noisy ) {
double actualError1 = factor->error(values);
SmartFactor::shared_ptr factor2(new SmartFactor(model, sharedK));
vector<Point2> measurements;
Point2Vector measurements;
measurements.push_back(level_uv);
measurements.push_back(level_uv_right);
@ -228,7 +228,7 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){
Point3 landmark2(5, -0.5, 1.2);
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
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@ -292,7 +292,7 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){
TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
using namespace vanillaPose2;
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
// Project three landmarks into three cameras
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@ -363,7 +363,7 @@ TEST( SmartProjectionPoseFactor, Factors ) {
// one landmarks 1m in front of camera
Point3 landmark1(0, 0, 10);
vector<Point2> measurements_cam1;
Point2Vector measurements_cam1;
// Project 2 landmarks into 2 cameras
measurements_cam1.push_back(cam1.project(landmark1));
@ -454,7 +454,7 @@ TEST( SmartProjectionPoseFactor, Factors ) {
E(2, 0) = 10;
E(2, 2) = 1;
E(3, 1) = 10;
vector<Matrix26> Fblocks = list_of<Matrix>(F1)(F2);
SmartFactor::FBlocks Fs = list_of<Matrix>(F1)(F2);
Vector b(4);
b.setZero();
@ -466,7 +466,7 @@ TEST( SmartProjectionPoseFactor, Factors ) {
// createJacobianQFactor
SharedIsotropic n = noiseModel::Isotropic::Sigma(4, sigma);
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));
boost::shared_ptr<JacobianFactorQ<6, 2> > actualQ =
@ -480,11 +480,11 @@ TEST( SmartProjectionPoseFactor, Factors ) {
// Whiten for RegularImplicitSchurFactor (does not have noise model)
model->WhitenSystem(E, b);
Matrix3 whiteP = (E.transpose() * E).inverse();
Fblocks[0] = model->Whiten(Fblocks[0]);
Fblocks[1] = model->Whiten(Fblocks[1]);
Fs[0] = model->Whiten(Fs[0]);
Fs[1] = model->Whiten(Fs[1]);
// createRegularImplicitSchurFactor
RegularImplicitSchurFactor<Camera> expected(keys, Fblocks, E, whiteP, b);
RegularImplicitSchurFactor<Camera> expected(keys, Fs, E, whiteP, b);
boost::shared_ptr<RegularImplicitSchurFactor<Camera> > actual =
smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0);
@ -525,7 +525,7 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
views.push_back(x2);
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
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@ -582,7 +582,7 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
views.push_back(x2);
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
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@ -643,7 +643,7 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) {
views.push_back(x2);
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
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@ -709,7 +709,7 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) {
// add fourth landmark
Point3 landmark4(5, -0.5, 1);
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3,
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3,
measurements_cam4;
// Project 4 landmarks into three cameras
@ -772,7 +772,7 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) {
views.push_back(x2);
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
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@ -883,7 +883,7 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
Camera cam2(pose2, 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
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 cam3(pose3, sharedK2);
vector<Point2> measurements_cam1, measurements_cam2;
Point2Vector measurements_cam1, measurements_cam2;
// Project three landmarks into three cameras
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@ -1026,7 +1026,7 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
Camera cam2(pose2, 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
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@ -1106,7 +1106,7 @@ TEST( SmartProjectionPoseFactor, Hessian ) {
// Project three landmarks into 2 cameras
Point2 cam1_uv1 = cam1.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(cam2_uv1);
@ -1138,7 +1138,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) {
views.push_back(x2);
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);
@ -1195,7 +1195,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) {
Camera cam2(level_pose, sharedK2);
Camera cam3(level_pose, sharedK2);
vector<Point2> measurements_cam1;
Point2Vector measurements_cam1;
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedK2));
@ -1253,7 +1253,7 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) {
// three landmarks ~5 meters in front of camera
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
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@ -1324,7 +1324,7 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
// landmark3 at 3 meters now
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
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@ -1422,7 +1422,7 @@ TEST(SmartProjectionPoseFactor, serialize2) {
// insert some measurments
vector<Key> key_view;
vector<Point2> meas_view;
Point2Vector meas_view;
key_view.push_back(Symbol('x', 1));
meas_view.push_back(Point2(10, 10));
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)
typedef PinholeCamera<Cal3_S2> MonoCamera;
typedef CameraSet<MonoCamera> MonoCameras;
typedef std::vector<Point2> MonoMeasurements;
typedef MonoCamera::MeasurementVector MonoMeasurements;
/**
* Constructor
@ -226,17 +226,17 @@ public:
}
// Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols().
std::vector<Base::MatrixZD> Fblocks;
Base::FBlocks Fs;
Matrix F, E;
Vector b;
computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras);
// Whiten using noise model
Base::whitenJacobians(Fblocks, E, b);
Base::whitenJacobians(Fs, E, b);
// build augmented hessian
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_,
augmentedHessian);
@ -360,7 +360,8 @@ public:
/// Assumes the point has been computed
/// Note E can be 2m*3 or 2m*2, in case point is degenerate
void computeJacobiansWithTriangulatedPoint(
std::vector<Base::MatrixZD>& Fblocks, Matrix& E, Vector& b,
FBlocks& Fs,
Matrix& E, Vector& b,
const Cameras& cameras) const {
if (!result_) {
@ -370,32 +371,32 @@ public:
// Unit3 backProjected; /* = cameras[0].backprojectPointAtInfinity(
// this->measured_.at(0)); */
//
// Base::computeJacobians(Fblocks, E, b, cameras, backProjected);
// Base::computeJacobians(Fs, E, b, cameras, backProjected);
} else {
// 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
bool triangulateAndComputeJacobians(
std::vector<Base::MatrixZD>& Fblocks, Matrix& E, Vector& b,
FBlocks& Fs, Matrix& E, Vector& b,
const Values& values) const {
Cameras cameras = this->cameras(values);
bool nonDegenerate = triangulateForLinearize(cameras);
if (nonDegenerate)
computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras);
return nonDegenerate;
}
/// takes values
bool triangulateAndComputeJacobiansSVD(
std::vector<Base::MatrixZD>& Fblocks, Matrix& Enull, Vector& b,
FBlocks& Fs, Matrix& Enull, Vector& b,
const Values& values) const {
Cameras cameras = this->cameras(values);
bool nonDegenerate = triangulateForLinearize(cameras);
if (nonDegenerate)
Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, *result_);
Base::computeJacobiansSVD(Fs, Enull, b, cameras, *result_);
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 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
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);