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

Merge remote-tracking branch 'origin/feature/small_Rot3_optimizations' into feature/ImuFactorPush2 

Conflicts:
	gtsam/geometry/SO3.cpp
	gtsam/navigation/ImuFactor.h
	gtsam/navigation/PreintegratedRotation.h
release/4.3a0
dellaert 2016-01-17 19:30:25 -08:00
parent be658119b9 a0f32f6d14
commit 73309d6fcf
17 changed files with 262 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/base/Matrix.cpp
View File

@ -580,15 +580,6 @@ Matrix vector_scale(const Matrix& A, const Vector& v, bool inf_mask) {
return M;
}
/* ************************************************************************* */
Matrix3 skewSymmetric(double wx, double wy, double wz)
{
return (Matrix3() <<
0.0, -wz, +wy,
+wz, 0.0, -wx,
-wy, +wx, 0.0).finished();
}
/* ************************************************************************* */
Matrix LLt(const Matrix& A)
{

12
gtsam/base/Matrix.h
View File

@ -477,9 +477,15 @@ GTSAM_EXPORT Matrix vector_scale(const Matrix& A, const Vector& v, bool inf_mask
* @param wz
* @return a 3*3 skew symmetric matrix
*/
GTSAM_EXPORT Matrix3 skewSymmetric(double wx, double wy, double wz);
template<class Derived>
inline Matrix3 skewSymmetric(const Eigen::MatrixBase<Derived>& w) { return skewSymmetric(w(0),w(1),w(2));}
inline Matrix3 skewSymmetric(double wx, double wy, double wz) {
return (Matrix3() << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0).finished();
}
template <class Derived>
inline Matrix3 skewSymmetric(const Eigen::MatrixBase<Derived>& w) {
return skewSymmetric(w(0), w(1), w(2));
}
/** Use Cholesky to calculate inverse square root of a matrix */
GTSAM_EXPORT Matrix inverse_square_root(const Matrix& A);

10
gtsam/geometry/Rot3Q.cpp
View File

@ -27,14 +27,12 @@ using namespace std;
namespace gtsam {
static const Matrix I3 = eye(3);
/* ************************************************************************* */
Rot3::Rot3() : quaternion_(Quaternion::Identity()) {}
/* ************************************************************************* */
Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) :
quaternion_((Eigen::Matrix3d() <<
quaternion_((Matrix3() <<
col1.x(), col2.x(), col3.x(),
col1.y(), col2.y(), col3.y(),
col1.z(), col2.z(), col3.z()).finished()) {}
@ -43,7 +41,7 @@ namespace gtsam {
Rot3::Rot3(double R11, double R12, double R13,
double R21, double R22, double R23,
double R31, double R32, double R33) :
quaternion_((Eigen::Matrix3d() <<
quaternion_((Matrix3() <<
R11, R12, R13,
R21, R22, R23,
R31, R32, R33).finished()) {}
@ -91,10 +89,10 @@ namespace gtsam {
/* ************************************************************************* */
Point3 Rot3::rotate(const Point3& p,
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
Matrix R = matrix();
const Matrix3 R = matrix();
if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z());
if (H2) *H2 = R;
Eigen::Vector3d r = R * p.vector();
const Vector3 r = R * p.vector();
return Point3(r.x(), r.y(), r.z());
}

15
gtsam/geometry/SO3.cpp
View File

@ -132,9 +132,9 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
using std::sin;
double theta2 = omega.dot(omega);
const double theta2 = omega.dot(omega);
if (theta2 <= std::numeric_limits<double>::epsilon()) return I_3x3;
double theta = std::sqrt(theta2); // rotation angle
const double theta = std::sqrt(theta2); // rotation angle
#ifdef DUY_VERSION
/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
Matrix3 X = skewSymmetric(omega);
@ -153,10 +153,15 @@ Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
* a perturbation on the manifold (expmap(Jr * omega))
*/
// element of Lie algebra so(3): X = omega^, normalized by normx
const Matrix3 Y = skewSymmetric(omega) / theta;
const double wx = omega.x(), wy = omega.y(), wz = omega.z();
Matrix3 Y;
Y << 0.0, -wz, +wy,
+wz, 0.0, -wx,
-wy, +wx, 0.0;
const double s2 = sin(theta / 2.0);
return I_3x3 - (2.0 * s2 * s2 / theta) * Y +
(1.0 - sin(theta) / theta) * Y * Y; // right Jacobian
const double a = (2.0 * s2 * s2 / theta2);
const double b = (1.0 - sin(theta) / theta) / theta2;
return I_3x3 - a * Y + b * Y * Y;
#endif
}

9
gtsam/geometry/triangulation.h
View File

@ -376,6 +376,15 @@ class TriangulationResult: public boost::optional<Point3> {
status_(s) {
}
public:
/**
* Default constructor, only for serialization
*/
TriangulationResult() {}
/**
* Constructor
*/
TriangulationResult(const Point3& p) :
status_(VALID) {
reset(p);

17
gtsam/navigation/ImuBias.h
View File

@ -78,19 +78,19 @@ public:
/** Correct an accelerometer measurement using this bias model, and optionally compute Jacobians */
Vector3 correctAccelerometer(const Vector3& measurement,
OptionalJacobian<3, 6> H = boost::none) const {
if (H) {
(*H) << -I_3x3, Z_3x3;
}
OptionalJacobian<3, 6> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const {
if (H1) (*H1) << -I_3x3, Z_3x3;
if (H2) (*H2) << I_3x3;
return measurement - biasAcc_;
}
/** Correct a gyroscope measurement using this bias model, and optionally compute Jacobians */
Vector3 correctGyroscope(const Vector3& measurement,
OptionalJacobian<3, 6> H = boost::none) const {
if (H) {
(*H) << Z_3x3, -I_3x3;
}
OptionalJacobian<3, 6> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const {
if (H1) (*H1) << Z_3x3, -I_3x3;
if (H2) (*H2) << I_3x3;
return measurement - biasGyro_;
}
@ -201,7 +201,6 @@ private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("imuBias::ConstantBias", *this);
ar & BOOST_SERIALIZATION_NVP(biasAcc_);
ar & BOOST_SERIALIZATION_NVP(biasGyro_);
}

3
gtsam/navigation/ImuFactor.h
View File

@ -140,8 +140,9 @@ private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
ar & BOOST_SERIALIZATION_NVP(preintMeasCov_);
ar & bs::make_nvp("preintMeasCov_", bs::make_array(preintMeasCov_.data(), preintMeasCov_.size()));
}
};

10
gtsam/navigation/PreintegratedRotation.h
View File

@ -47,10 +47,12 @@ class PreintegratedRotation {
private:
/** Serialization function */
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_NVP(gyroscopeCovariance);
ar& BOOST_SERIALIZATION_NVP(omegaCoriolis);
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar & bs::make_nvp("gyroscopeCovariance", bs::make_array(gyroscopeCovariance.data(), gyroscopeCovariance.size()));
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
ar& BOOST_SERIALIZATION_NVP(body_P_sensor);
}
};

21
gtsam/navigation/PreintegrationBase.h
View File

@ -86,15 +86,17 @@ public:
protected:
/// Default constructor for serialization only: uninitialized!
Params();
Params() {}
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation::Params);
ar & BOOST_SERIALIZATION_NVP(accelerometerCovariance);
ar & BOOST_SERIALIZATION_NVP(integrationCovariance);
namespace bs = ::boost::serialization;
ar & boost::serialization::make_nvp("PreintegratedRotation_Params",
boost::serialization::base_object<PreintegratedRotation::Params>(*this));
ar & bs::make_nvp("accelerometerCovariance", bs::make_array(accelerometerCovariance.data(), accelerometerCovariance.size()));
ar & bs::make_nvp("integrationCovariance", bs::make_array(integrationCovariance.data(), integrationCovariance.size()));
ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis);
ar & BOOST_SERIALIZATION_NVP(n_gravity);
}
@ -300,15 +302,16 @@ private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar & BOOST_SERIALIZATION_NVP(p_);
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
ar & BOOST_SERIALIZATION_NVP(deltaXij_);
ar & BOOST_SERIALIZATION_NVP(biasHat_);
ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_);
ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_);
ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_);
ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_);
ar & bs::make_nvp("delRdelBiasOmega_", bs::make_array(delRdelBiasOmega_.data(), delRdelBiasOmega_.size()));
ar & bs::make_nvp("delPdelBiasAcc_", bs::make_array(delPdelBiasAcc_.data(), delPdelBiasAcc_.size()));
ar & bs::make_nvp("delPdelBiasOmega_", bs::make_array(delPdelBiasOmega_.data(), delPdelBiasOmega_.size()));
ar & bs::make_nvp("delVdelBiasAcc_", bs::make_array(delVdelBiasAcc_.data(), delVdelBiasAcc_.size()));
ar & bs::make_nvp("delVdelBiasOmega_", bs::make_array(delVdelBiasOmega_.data(), delVdelBiasOmega_.size()));
}
};

102
gtsam/navigation/tests/testImuBias.cpp
View File

@ -16,113 +16,135 @@
*/
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
using namespace std;
using namespace gtsam;
typedef imuBias::ConstantBias Bias;
Vector biasAcc1(Vector3(0.2, -0.1, 0));
Vector biasGyro1(Vector3(0.1, -0.3, -0.2));
imuBias::ConstantBias bias1(biasAcc1, biasGyro1);
Bias bias1(biasAcc1, biasGyro1);
Vector biasAcc2(Vector3(0.1, 0.2, 0.04));
Vector biasGyro2(Vector3(-0.002, 0.005, 0.03));
imuBias::ConstantBias bias2(biasAcc2, biasGyro2);
Bias bias2(biasAcc2, biasGyro2);
/* ************************************************************************* */
TEST( ImuBias, Constructor) {
TEST(ImuBias, Constructor) {
// Default Constructor
imuBias::ConstantBias bias1;
Bias bias1;
// Acc + Gyro Constructor
imuBias::ConstantBias bias2(biasAcc2, biasGyro2);
Bias bias2(biasAcc2, biasGyro2);
// Copy Constructor
imuBias::ConstantBias bias3(bias2);
Bias bias3(bias2);
}
/* ************************************************************************* */
TEST( ImuBias, inverse) {
imuBias::ConstantBias biasActual = bias1.inverse();
imuBias::ConstantBias biasExpected = imuBias::ConstantBias(-biasAcc1,
-biasGyro1);
TEST(ImuBias, inverse) {
Bias biasActual = bias1.inverse();
Bias biasExpected = Bias(-biasAcc1, -biasGyro1);
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
TEST( ImuBias, compose) {
imuBias::ConstantBias biasActual = bias2.compose(bias1);
imuBias::ConstantBias biasExpected = imuBias::ConstantBias(
biasAcc1 + biasAcc2, biasGyro1 + biasGyro2);
TEST(ImuBias, compose) {
Bias biasActual = bias2.compose(bias1);
Bias biasExpected = Bias(biasAcc1 + biasAcc2, biasGyro1 + biasGyro2);
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
TEST( ImuBias, between) {
TEST(ImuBias, between) {
// p.between(q) == q - p
imuBias::ConstantBias biasActual = bias2.between(bias1);
imuBias::ConstantBias biasExpected = imuBias::ConstantBias(
biasAcc1 - biasAcc2, biasGyro1 - biasGyro2);
Bias biasActual = bias2.between(bias1);
Bias biasExpected = Bias(biasAcc1 - biasAcc2, biasGyro1 - biasGyro2);
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
TEST( ImuBias, localCoordinates) {
TEST(ImuBias, localCoordinates) {
Vector deltaActual = Vector(bias2.localCoordinates(bias1));
Vector deltaExpected = (imuBias::ConstantBias(biasAcc1 - biasAcc2,
biasGyro1 - biasGyro2)).vector();
Vector deltaExpected =
(Bias(biasAcc1 - biasAcc2, biasGyro1 - biasGyro2)).vector();
EXPECT(assert_equal(deltaExpected, deltaActual));
}
/* ************************************************************************* */
TEST( ImuBias, retract) {
TEST(ImuBias, retract) {
Vector6 delta;
delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2;
imuBias::ConstantBias biasActual = bias2.retract(delta);
imuBias::ConstantBias biasExpected = imuBias::ConstantBias(
biasAcc2 + delta.block<3, 1>(0, 0), biasGyro2 + delta.block<3, 1>(3, 0));
Bias biasActual = bias2.retract(delta);
Bias biasExpected = Bias(biasAcc2 + delta.block<3, 1>(0, 0),
biasGyro2 + delta.block<3, 1>(3, 0));
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
TEST( ImuBias, Logmap) {
TEST(ImuBias, Logmap) {
Vector deltaActual = bias2.Logmap(bias1);
Vector deltaExpected = bias1.vector();
EXPECT(assert_equal(deltaExpected, deltaActual));
}
/* ************************************************************************* */
TEST( ImuBias, Expmap) {
TEST(ImuBias, Expmap) {
Vector6 delta;
delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2;
imuBias::ConstantBias biasActual = bias2.Expmap(delta);
imuBias::ConstantBias biasExpected = imuBias::ConstantBias(delta);
Bias biasActual = bias2.Expmap(delta);
Bias biasExpected = Bias(delta);
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
TEST( ImuBias, operatorSub) {
imuBias::ConstantBias biasActual = -bias1;
imuBias::ConstantBias biasExpected(-biasAcc1, -biasGyro1);
TEST(ImuBias, operatorSub) {
Bias biasActual = -bias1;
Bias biasExpected(-biasAcc1, -biasGyro1);
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
TEST( ImuBias, operatorAdd) {
imuBias::ConstantBias biasActual = bias2 + bias1;
imuBias::ConstantBias biasExpected(biasAcc2 + biasAcc1,
biasGyro2 + biasGyro1);
TEST(ImuBias, operatorAdd) {
Bias biasActual = bias2 + bias1;
Bias biasExpected(biasAcc2 + biasAcc1, biasGyro2 + biasGyro1);
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
TEST( ImuBias, operatorSubB) {
imuBias::ConstantBias biasActual = bias2 - bias1;
imuBias::ConstantBias biasExpected(biasAcc2 - biasAcc1,
biasGyro2 - biasGyro1);
TEST(ImuBias, operatorSubB) {
Bias biasActual = bias2 - bias1;
Bias biasExpected(biasAcc2 - biasAcc1, biasGyro2 - biasGyro1);
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
TEST(ImuBias, Correct1) {
Matrix aH1, aH2;
const Vector3 measurement(1, 2, 3);
boost::function<Vector3(const Bias&, const Vector3&)> f = boost::bind(
&Bias::correctAccelerometer, _1, _2, boost::none, boost::none);
bias1.correctAccelerometer(measurement, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1));
EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2));
}
/* ************************************************************************* */
TEST(ImuBias, Correct2) {
Matrix aH1, aH2;
const Vector3 measurement(1, 2, 3);
boost::function<Vector3(const Bias&, const Vector3&)> f =
boost::bind(&Bias::correctGyroscope, _1, _2, boost::none, boost::none);
bias1.correctGyroscope(measurement, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1));
EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2));
}
/* ************************************************************************* */
int main() {
TestResult tr;

46
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -859,6 +859,52 @@ TEST(ImuFactor, bodyPSensorWithBias) {
EXPECT(assert_equal(biasExpected, biasActual, 1e-3));
}
/* ************************************************************************** */
#include <gtsam/base/serializationTestHelpers.h>
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
TEST(ImuFactor, serialization) {
using namespace gtsam::serializationTestHelpers;
Vector3 n_gravity(0, 0, -9.81);
Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3());
Matrix3 accCov = 1e-7 * I_3x3;
Matrix3 gyroCov = 1e-8 * I_3x3;
Matrix3 integrationCov = 1e-9 * I_3x3;
double deltaT = 0.005;
imuBias::ConstantBias priorBias(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); // Biases (acc, rot)
ImuFactor::PreintegratedMeasurements pim =
ImuFactor::PreintegratedMeasurements(priorBias, accCov, gyroCov,
integrationCov, true);
// measurements are needed for non-inf noise model, otherwise will throw err when deserialize
// Sensor frame is z-down
// Gyroscope measurement is the angular velocity of sensor w.r.t nav frame in sensor frame
Vector3 measuredOmega(0, 0.01, 0);
// Acc measurement is acceleration of sensor in the sensor frame, when stationary,
// table exerts an equal and opposite force w.r.t gravity
Vector3 measuredAcc(0, 0, -9.81);
for (int j = 0; j < 200; ++j)
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT,
body_P_sensor);
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim);
EXPECT(equalsObj(factor));
EXPECT(equalsXML(factor));
EXPECT(equalsBinary(factor));
}
/* ************************************************************************* */
int main() {
TestResult tr;

3
gtsam/slam/SmartFactorBase.h
View File

@ -93,6 +93,9 @@ public:
/// We use the new CameraSte data structure to refer to a set of cameras
typedef CameraSet<CAMERA> Cameras;
/// Default Constructor, for serialization
SmartFactorBase() {}
/// Constructor
SmartFactorBase(const SharedNoiseModel& sharedNoiseModel,
boost::optional<Pose3> body_P_sensor = boost::none) :

7
gtsam/slam/SmartProjectionFactor.h
View File

@ -137,7 +137,7 @@ protected:
/// @name Parameters
/// @{
const SmartProjectionParams params_;
SmartProjectionParams params_;
/// @}
/// @name Caching triangulation
@ -154,6 +154,11 @@ public:
/// shorthand for a set of cameras
typedef CameraSet<CAMERA> Cameras;
/**
* Default constructor, only for serialization
*/
SmartProjectionFactor() {}
/**
* Constructor
* @param body_P_sensor pose of the camera in the body frame

5
gtsam/slam/SmartProjectionPoseFactor.h
View File

@ -59,6 +59,11 @@ public:
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
/**
* Default constructor, only for serialization
*/
SmartProjectionPoseFactor() {}
/**
* Constructor
* @param Isotropic measurement noise

37
gtsam/slam/tests/testSmartFactorBase.cpp
View File

@ -18,6 +18,7 @@
#include <gtsam/slam/SmartFactorBase.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
@ -29,9 +30,13 @@ static SharedNoiseModel unit3(noiseModel::Unit::Create(3));
/* ************************************************************************* */
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3Bundler.h>
namespace gtsam {
class PinholeFactor: public SmartFactorBase<PinholeCamera<Cal3Bundler> > {
public:
typedef SmartFactorBase<PinholeCamera<Cal3Bundler> > Base;
PinholeFactor() {}
PinholeFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) {
}
virtual double error(const Values& values) const {
@ -43,6 +48,11 @@ public:
}
};
/// traits
template<>
struct traits<PinholeFactor> : public Testable<PinholeFactor> {};
}
TEST(SmartFactorBase, Pinhole) {
PinholeFactor f= PinholeFactor(unit2);
f.add(Point2(), 1);
@ -52,9 +62,13 @@ TEST(SmartFactorBase, Pinhole) {
/* ************************************************************************* */
#include <gtsam/geometry/StereoCamera.h>
namespace gtsam {
class StereoFactor: public SmartFactorBase<StereoCamera> {
public:
typedef SmartFactorBase<StereoCamera> Base;
StereoFactor() {}
StereoFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) {
}
virtual double error(const Values& values) const {
@ -66,6 +80,11 @@ public:
}
};
/// traits
template<>
struct traits<StereoFactor> : public Testable<StereoFactor> {};
}
TEST(SmartFactorBase, Stereo) {
StereoFactor f(unit3);
f.add(StereoPoint2(), 1);
@ -73,6 +92,24 @@ TEST(SmartFactorBase, Stereo) {
EXPECT_LONGS_EQUAL(2 * 3, f.dim());
}
/* ************************************************************************* */
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
TEST(SmartFactorBase, serialize) {
using namespace gtsam::serializationTestHelpers;
PinholeFactor factor(unit2);
EXPECT(equalsObj(factor));
EXPECT(equalsXML(factor));
EXPECT(equalsBinary(factor));
}
/* ************************************************************************* */
int main() {
TestResult tr;

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

@ -22,6 +22,7 @@
#include "smartFactorScenarios.h"
#include <gtsam/slam/SmartProjectionFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/map.hpp>
#include <iostream>
@ -843,6 +844,26 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) {
EXPECT(assert_equal(yActual, yExpected, 1e-7));
}
/* ************************************************************************* */
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
TEST( SmartProjectionCameraFactor, serialize) {
using namespace vanilla;
using namespace gtsam::serializationTestHelpers;
SmartFactor factor(unit2);
EXPECT(equalsObj(factor));
EXPECT(equalsXML(factor));
EXPECT(equalsBinary(factor));
}
/* ************************************************************************* */
int main() {
TestResult tr;

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

@ -24,6 +24,7 @@
#include <gtsam/slam/PoseTranslationPrior.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/map.hpp>
#include <iostream>
@ -1387,6 +1388,27 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
values.at<Pose3>(x3)));
}
/* ************************************************************************* */
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
TEST(SmartProjectionPoseFactor, serialize) {
using namespace vanillaPose;
using namespace gtsam::serializationTestHelpers;
SmartProjectionParams params;
params.setRankTolerance(rankTol);
SmartFactor factor(model, sharedK, boost::none, params);
EXPECT(equalsObj(factor));
EXPECT(equalsXML(factor));
EXPECT(equalsBinary(factor));
}
/* ************************************************************************* */
int main() {
TestResult tr;