Merged gtborg/gtsam into develop
Mike Sheffler
commit
b0097b7751
|
|
@ -62,10 +62,11 @@ If you are using the factor in academic work, please cite the publications above
|
|||
In GTSAM 4 a new and more efficient implementation, based on integrating on the NavState tangent space and detailed in docs/ImuFactor.pdf, is enabled by default. To switch to the RSS 2015 version, set the flag **GTSAM_TANGENT_PREINTEGRATION** to OFF.
|
||||
|
||||
|
||||
|
||||
Additional Information
|
||||
----------------------
|
||||
|
||||
There is a [`GTSAM users Google group`](https://groups.google.com/forum/#!forum/gtsam-users) for general discussion.
|
||||
|
||||
Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here. A primer on GTSAM Expressions,
|
||||
which support (superfast) automatic differentiation,
|
||||
can be found on the [GTSAM wiki on BitBucket](https://bitbucket.org/gtborg/gtsam/wiki/Home).
|
||||
|
|
|
|||
10
gtsam.h
10
gtsam.h
|
|
@ -969,9 +969,10 @@ virtual class SimpleCamera {
|
|||
// Some typedefs for common camera types
|
||||
// PinholeCameraCal3_S2 is the same as SimpleCamera above
|
||||
typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
|
||||
typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
|
||||
typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
|
||||
typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
|
||||
// due to lack of jacobians of Cal3DS2_Base::calibrate, PinholeCamera does not apply to Cal3DS2/Unified
|
||||
//typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
|
||||
//typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
|
||||
//typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
|
||||
|
||||
#include <gtsam/geometry/StereoCamera.h>
|
||||
class StereoCamera {
|
||||
|
|
@ -2337,7 +2338,8 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
|
|||
gtsam::Point2 measured() const;
|
||||
};
|
||||
typedef gtsam::GeneralSFMFactor<gtsam::SimpleCamera, gtsam::Point3> GeneralSFMFactorCal3_S2;
|
||||
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
|
||||
// due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2
|
||||
//typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
|
||||
|
||||
template<CALIBRATION = {gtsam::Cal3_S2}>
|
||||
virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
|
||||
|
|
|
|||
|
|
@ -144,8 +144,9 @@ bool choleskyPartial(Matrix& ABC, size_t nFrontal, size_t topleft) {
|
|||
// Check last diagonal element - Eigen does not check it
|
||||
if (nFrontal >= 2) {
|
||||
int exp2, exp1;
|
||||
(void)frexp(R(topleft + nFrontal - 2, topleft + nFrontal - 2), &exp2);
|
||||
(void)frexp(R(topleft + nFrontal - 1, topleft + nFrontal - 1), &exp1);
|
||||
// NOTE(gareth): R is already the size of A, so we don't need to add topleft here.
|
||||
(void)frexp(R(nFrontal - 2, nFrontal - 2), &exp2);
|
||||
(void)frexp(R(nFrontal - 1, nFrontal - 1), &exp1);
|
||||
return (exp2 - exp1 < underconstrainedExponentDifference);
|
||||
} else if (nFrontal == 1) {
|
||||
int exp1;
|
||||
|
|
|
|||
|
|
@ -298,7 +298,7 @@ inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(Y (*
|
|||
|
||||
/**
|
||||
* Compute numerical derivative in argument 1 of 4-argument function
|
||||
* @param h ternary function yielding m-vector
|
||||
* @param h quartic function yielding m-vector
|
||||
* @param x1 n-dimensional first argument value
|
||||
* @param x2 second argument value
|
||||
* @param x3 third argument value
|
||||
|
|
@ -317,9 +317,15 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(
|
|||
return numericalDerivative11<Y, X1>(boost::bind(h, _1, x2, x3, x4), x1, delta);
|
||||
}
|
||||
|
||||
template<class Y, class X1, class X2, class X3, class X4>
|
||||
inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(Y (*h)(const X1&, const X2&, const X3&, const X4&),
|
||||
const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
|
||||
return numericalDerivative41<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4);
|
||||
}
|
||||
|
||||
/**
|
||||
* Compute numerical derivative in argument 2 of 4-argument function
|
||||
* @param h ternary function yielding m-vector
|
||||
* @param h quartic function yielding m-vector
|
||||
* @param x1 first argument value
|
||||
* @param x2 n-dimensional second argument value
|
||||
* @param x3 third argument value
|
||||
|
|
@ -338,9 +344,15 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(
|
|||
return numericalDerivative11<Y, X2>(boost::bind(h, x1, _1, x3, x4), x2, delta);
|
||||
}
|
||||
|
||||
template<class Y, class X1, class X2, class X3, class X4>
|
||||
inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(Y (*h)(const X1&, const X2&, const X3&, const X4&),
|
||||
const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
|
||||
return numericalDerivative42<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4);
|
||||
}
|
||||
|
||||
/**
|
||||
* Compute numerical derivative in argument 3 of 4-argument function
|
||||
* @param h ternary function yielding m-vector
|
||||
* @param h quartic function yielding m-vector
|
||||
* @param x1 first argument value
|
||||
* @param x2 second argument value
|
||||
* @param x3 n-dimensional third argument value
|
||||
|
|
@ -359,9 +371,15 @@ typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(
|
|||
return numericalDerivative11<Y, X3>(boost::bind(h, x1, x2, _1, x4), x3, delta);
|
||||
}
|
||||
|
||||
template<class Y, class X1, class X2, class X3, class X4>
|
||||
inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(Y (*h)(const X1&, const X2&, const X3&, const X4&),
|
||||
const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
|
||||
return numericalDerivative43<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4);
|
||||
}
|
||||
|
||||
/**
|
||||
* Compute numerical derivative in argument 4 of 4-argument function
|
||||
* @param h ternary function yielding m-vector
|
||||
* @param h quartic function yielding m-vector
|
||||
* @param x1 first argument value
|
||||
* @param x2 second argument value
|
||||
* @param x3 third argument value
|
||||
|
|
@ -380,6 +398,152 @@ typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(
|
|||
return numericalDerivative11<Y, X4>(boost::bind(h, x1, x2, x3, _1), x4, delta);
|
||||
}
|
||||
|
||||
template<class Y, class X1, class X2, class X3, class X4>
|
||||
inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(Y (*h)(const X1&, const X2&, const X3&, const X4&),
|
||||
const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
|
||||
return numericalDerivative44<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4);
|
||||
}
|
||||
|
||||
/**
|
||||
* Compute numerical derivative in argument 1 of 5-argument function
|
||||
* @param h quintic function yielding m-vector
|
||||
* @param x1 n-dimensional first argument value
|
||||
* @param x2 second argument value
|
||||
* @param x3 third argument value
|
||||
* @param x4 fourth argument value
|
||||
* @param x5 fifth argument value
|
||||
* @param delta increment for numerical derivative
|
||||
* @return m*n Jacobian computed via central differencing
|
||||
*/
|
||||
template<class Y, class X1, class X2, class X3, class X4, class X5>
|
||||
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(
|
||||
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
|
||||
const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
|
||||
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
|
||||
"Template argument Y must be a manifold type.");
|
||||
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
|
||||
"Template argument X1 must be a manifold type.");
|
||||
return numericalDerivative11<Y, X1>(boost::bind(h, _1, x2, x3, x4, x5), x1, delta);
|
||||
}
|
||||
|
||||
template<class Y, class X1, class X2, class X3, class X4, class X5>
|
||||
inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
|
||||
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
|
||||
return numericalDerivative51<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
|
||||
}
|
||||
|
||||
/**
|
||||
* Compute numerical derivative in argument 2 of 5-argument function
|
||||
* @param h quintic function yielding m-vector
|
||||
* @param x1 n-dimensional first argument value
|
||||
* @param x2 second argument value
|
||||
* @param x3 third argument value
|
||||
* @param x4 fourth argument value
|
||||
* @param x5 fifth argument value
|
||||
* @param delta increment for numerical derivative
|
||||
* @return m*n Jacobian computed via central differencing
|
||||
*/
|
||||
template<class Y, class X1, class X2, class X3, class X4, class X5>
|
||||
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(
|
||||
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
|
||||
const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
|
||||
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
|
||||
"Template argument Y must be a manifold type.");
|
||||
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
|
||||
"Template argument X1 must be a manifold type.");
|
||||
return numericalDerivative11<Y, X2>(boost::bind(h, x1, _1, x3, x4, x5), x2, delta);
|
||||
}
|
||||
|
||||
template<class Y, class X1, class X2, class X3, class X4, class X5>
|
||||
inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative51(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
|
||||
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
|
||||
return numericalDerivative52<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
|
||||
}
|
||||
|
||||
/**
|
||||
* Compute numerical derivative in argument 3 of 5-argument function
|
||||
* @param h quintic function yielding m-vector
|
||||
* @param x1 n-dimensional first argument value
|
||||
* @param x2 second argument value
|
||||
* @param x3 third argument value
|
||||
* @param x4 fourth argument value
|
||||
* @param x5 fifth argument value
|
||||
* @param delta increment for numerical derivative
|
||||
* @return m*n Jacobian computed via central differencing
|
||||
*/
|
||||
template<class Y, class X1, class X2, class X3, class X4, class X5>
|
||||
typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(
|
||||
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
|
||||
const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
|
||||
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
|
||||
"Template argument Y must be a manifold type.");
|
||||
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
|
||||
"Template argument X1 must be a manifold type.");
|
||||
return numericalDerivative11<Y, X3>(boost::bind(h, x1, x2, _1, x4, x5), x3, delta);
|
||||
}
|
||||
|
||||
template<class Y, class X1, class X2, class X3, class X4, class X5>
|
||||
inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
|
||||
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
|
||||
return numericalDerivative53<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
|
||||
}
|
||||
|
||||
/**
|
||||
* Compute numerical derivative in argument 4 of 5-argument function
|
||||
* @param h quintic function yielding m-vector
|
||||
* @param x1 n-dimensional first argument value
|
||||
* @param x2 second argument value
|
||||
* @param x3 third argument value
|
||||
* @param x4 fourth argument value
|
||||
* @param x5 fifth argument value
|
||||
* @param delta increment for numerical derivative
|
||||
* @return m*n Jacobian computed via central differencing
|
||||
*/
|
||||
template<class Y, class X1, class X2, class X3, class X4, class X5>
|
||||
typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(
|
||||
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
|
||||
const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
|
||||
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
|
||||
"Template argument Y must be a manifold type.");
|
||||
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
|
||||
"Template argument X1 must be a manifold type.");
|
||||
return numericalDerivative11<Y, X4>(boost::bind(h, x1, x2, x3, _1, x5), x4, delta);
|
||||
}
|
||||
|
||||
template<class Y, class X1, class X2, class X3, class X4, class X5>
|
||||
inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative53(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
|
||||
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
|
||||
return numericalDerivative54<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
|
||||
}
|
||||
|
||||
/**
|
||||
* Compute numerical derivative in argument 5 of 5-argument function
|
||||
* @param h quintic function yielding m-vector
|
||||
* @param x1 n-dimensional first argument value
|
||||
* @param x2 second argument value
|
||||
* @param x3 third argument value
|
||||
* @param x4 fourth argument value
|
||||
* @param x5 fifth argument value
|
||||
* @param delta increment for numerical derivative
|
||||
* @return m*n Jacobian computed via central differencing
|
||||
*/
|
||||
template<class Y, class X1, class X2, class X3, class X4, class X5>
|
||||
typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(
|
||||
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
|
||||
const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
|
||||
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
|
||||
"Template argument Y must be a manifold type.");
|
||||
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
|
||||
"Template argument X1 must be a manifold type.");
|
||||
return numericalDerivative11<Y, X5>(boost::bind(h, x1, x2, x3, x4, _1), x5, delta);
|
||||
}
|
||||
|
||||
template<class Y, class X1, class X2, class X3, class X4, class X5>
|
||||
inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative53(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
|
||||
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
|
||||
return numericalDerivative55<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
|
||||
}
|
||||
|
||||
/**
|
||||
* Compute numerical Hessian matrix. Requires a single-argument Lie->scalar
|
||||
* function. This is implemented simply as the derivative of the gradient.
|
||||
|
|
|
|||
|
|
@ -24,6 +24,9 @@
|
|||
|
||||
#include <gtsam/base/serialization.h>
|
||||
|
||||
#include <boost/serialization/serialization.hpp>
|
||||
|
||||
|
||||
// whether to print the serialized text to stdout
|
||||
const bool verbose = false;
|
||||
|
||||
|
|
@ -142,4 +145,3 @@ bool equalsDereferencedBinary(const T& input = T()) {
|
|||
|
||||
} // \namespace serializationTestHelpers
|
||||
} // \namespace gtsam
|
||||
|
||||
|
|
|
|||
|
|
@ -142,8 +142,11 @@ Point2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose,
|
|||
Matrix23 Dpc_rot;
|
||||
Matrix2 Dpc_point;
|
||||
const Unit3 pc = pose().rotation().unrotate(pw, Dpose ? &Dpc_rot : 0,
|
||||
Dpose ? &Dpc_point : 0);
|
||||
|
||||
Dpoint ? &Dpc_point : 0);
|
||||
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
|
||||
if (pc.unitVector().z() <= 0)
|
||||
throw CheiralityException();
|
||||
#endif
|
||||
// camera to normalized image coordinate
|
||||
Matrix2 Dpn_pc;
|
||||
const Point2 pn = Project(pc, Dpose || Dpoint ? &Dpn_pc : 0);
|
||||
|
|
@ -161,8 +164,12 @@ Point2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose,
|
|||
return pn;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Point3 PinholeBase::backproject_from_camera(const Point2& p,
|
||||
const double depth) {
|
||||
Point3 PinholeBase::BackprojectFromCamera(const Point2& p,
|
||||
const double depth, OptionalJacobian<3, 2> Dpoint, OptionalJacobian<3, 1> Ddepth) {
|
||||
if (Dpoint)
|
||||
*Dpoint << depth, 0, 0, depth, 0, 0;
|
||||
if (Ddepth)
|
||||
*Ddepth << p.x(), p.y(), 1;
|
||||
return Point3(p.x() * depth, p.y() * depth, depth);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -201,7 +201,9 @@ public:
|
|||
OptionalJacobian<2, 2> Dpoint = boost::none) const;
|
||||
|
||||
/// backproject a 2-dimensional point to a 3-dimensional point at given depth
|
||||
static Point3 backproject_from_camera(const Point2& p, const double depth);
|
||||
static Point3 BackprojectFromCamera(const Point2& p, const double depth,
|
||||
OptionalJacobian<3, 2> Dpoint = boost::none,
|
||||
OptionalJacobian<3, 1> Ddepth = boost::none);
|
||||
|
||||
/// @}
|
||||
/// @name Advanced interface
|
||||
|
|
@ -337,8 +339,28 @@ public:
|
|||
boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const;
|
||||
|
||||
/// backproject a 2-dimensional point to a 3-dimensional point at given depth
|
||||
Point3 backproject(const Point2& pn, double depth) const {
|
||||
return pose().transform_from(backproject_from_camera(pn, depth));
|
||||
Point3 backproject(const Point2& pn, double depth,
|
||||
OptionalJacobian<3, 6> Dresult_dpose = boost::none,
|
||||
OptionalJacobian<3, 2> Dresult_dp = boost::none,
|
||||
OptionalJacobian<3, 1> Dresult_ddepth = boost::none) const {
|
||||
|
||||
Matrix32 Dpoint_dpn;
|
||||
Matrix31 Dpoint_ddepth;
|
||||
const Point3 point = BackprojectFromCamera(pn, depth,
|
||||
Dresult_dp ? &Dpoint_dpn : 0,
|
||||
Dresult_ddepth ? &Dpoint_ddepth : 0);
|
||||
|
||||
Matrix33 Dresult_dpoint;
|
||||
const Point3 result = pose().transform_from(point, Dresult_dpose,
|
||||
(Dresult_ddepth ||
|
||||
Dresult_dp) ? &Dresult_dpoint : 0);
|
||||
|
||||
if (Dresult_dp)
|
||||
*Dresult_dp = Dresult_dpoint * Dpoint_dpn;
|
||||
if (Dresult_ddepth)
|
||||
*Dresult_ddepth = Dresult_dpoint * Dpoint_ddepth;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -404,4 +426,3 @@ template <typename T>
|
|||
struct Range<CalibratedCamera, T> : HasRange<CalibratedCamera, T, double> {};
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
|
|
|
|||
|
|
@ -85,25 +85,9 @@ public:
|
|||
return pn;
|
||||
}
|
||||
|
||||
/** project a point from world coordinate to the image
|
||||
* @param pw is a point in the world coordinates
|
||||
*/
|
||||
Point2 project(const Point3& pw) const {
|
||||
const Point2 pn = PinholeBase::project2(pw); // project to normalized coordinates
|
||||
return calibration().uncalibrate(pn); // uncalibrate to pixel coordinates
|
||||
}
|
||||
|
||||
/** project a point from world coordinate to the image
|
||||
* @param pw is a point at infinity in the world coordinates
|
||||
*/
|
||||
Point2 project(const Unit3& pw) const {
|
||||
const Unit3 pc = pose().rotation().unrotate(pw); // convert to camera frame
|
||||
const Point2 pn = PinholeBase::Project(pc); // project to normalized coordinates
|
||||
return calibration().uncalibrate(pn); // uncalibrate to pixel coordinates
|
||||
}
|
||||
|
||||
/** Templated projection of a point (possibly at infinity) from world coordinate to the image
|
||||
* @param pw is a 3D point or aUnit3 (point at infinity) in world coordinates
|
||||
* @param pw is a 3D point or a Unit3 (point at infinity) in world coordinates
|
||||
* @param Dpose is the Jacobian w.r.t. pose3
|
||||
* @param Dpoint is the Jacobian w.r.t. point3
|
||||
* @param Dcal is the Jacobian w.r.t. calibration
|
||||
|
|
@ -131,25 +115,51 @@ public:
|
|||
}
|
||||
|
||||
/// project a 3D point from world coordinates into the image
|
||||
Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose,
|
||||
Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
|
||||
OptionalJacobian<2, 3> Dpoint = boost::none,
|
||||
OptionalJacobian<2, DimK> Dcal = boost::none) const {
|
||||
return _project(pw, Dpose, Dpoint, Dcal);
|
||||
}
|
||||
|
||||
/// project a point at infinity from world coordinates into the image
|
||||
Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose,
|
||||
Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
|
||||
OptionalJacobian<2, 2> Dpoint = boost::none,
|
||||
OptionalJacobian<2, DimK> Dcal = boost::none) const {
|
||||
return _project(pw, Dpose, Dpoint, Dcal);
|
||||
}
|
||||
|
||||
/// backproject a 2-dimensional point to a 3-dimensional point at given depth
|
||||
Point3 backproject(const Point2& p, double depth) const {
|
||||
const Point2 pn = calibration().calibrate(p);
|
||||
return pose().transform_from(backproject_from_camera(pn, depth));
|
||||
Point3 backproject(const Point2& p, double depth,
|
||||
OptionalJacobian<3, 6> Dresult_dpose = boost::none,
|
||||
OptionalJacobian<3, 2> Dresult_dp = boost::none,
|
||||
OptionalJacobian<3, 1> Dresult_ddepth = boost::none,
|
||||
OptionalJacobian<3, DimK> Dresult_dcal = boost::none) const {
|
||||
typedef Eigen::Matrix<double, 2, DimK> Matrix2K;
|
||||
Matrix2K Dpn_dcal;
|
||||
Matrix22 Dpn_dp;
|
||||
const Point2 pn = calibration().calibrate(p, Dresult_dcal ? &Dpn_dcal : 0,
|
||||
Dresult_dp ? &Dpn_dp : 0);
|
||||
Matrix32 Dpoint_dpn;
|
||||
Matrix31 Dpoint_ddepth;
|
||||
const Point3 point = BackprojectFromCamera(pn, depth,
|
||||
(Dresult_dp || Dresult_dcal) ? &Dpoint_dpn : 0,
|
||||
Dresult_ddepth ? &Dpoint_ddepth : 0);
|
||||
Matrix33 Dresult_dpoint;
|
||||
const Point3 result = pose().transform_from(point, Dresult_dpose,
|
||||
(Dresult_ddepth ||
|
||||
Dresult_dp ||
|
||||
Dresult_dcal) ? &Dresult_dpoint : 0);
|
||||
if (Dresult_dcal)
|
||||
*Dresult_dcal = Dresult_dpoint * Dpoint_dpn * Dpn_dcal; // (3x3)*(3x2)*(2xDimK)
|
||||
if (Dresult_dp)
|
||||
*Dresult_dp = Dresult_dpoint * Dpoint_dpn * Dpn_dp; // (3x3)*(3x2)*(2x2)
|
||||
if (Dresult_ddepth)
|
||||
*Dresult_ddepth = Dresult_dpoint * Dpoint_ddepth; // (3x3)*(3x1)
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
/// backproject a 2-dimensional point to a 3-dimensional point at infinity
|
||||
Unit3 backprojectPointAtInfinity(const Point2& p) const {
|
||||
const Point2 pn = calibration().calibrate(p);
|
||||
|
|
@ -302,6 +312,11 @@ public:
|
|||
Base(v), K_(new CALIBRATION(K)) {
|
||||
}
|
||||
|
||||
// Init from Pose3 and calibration
|
||||
PinholePose(const Pose3 &pose, const Vector &K) :
|
||||
Base(pose), K_(new CALIBRATION(K)) {
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
|
|
|||
|
|
@ -113,7 +113,7 @@ static Point2 Project2(const Unit3& point) {
|
|||
return PinholeBase::Project(point);
|
||||
}
|
||||
|
||||
Unit3 pointAtInfinity(0, 0, 1000);
|
||||
Unit3 pointAtInfinity(0, 0, -1000);
|
||||
TEST( CalibratedCamera, DProjectInfinity) {
|
||||
Matrix test1;
|
||||
CalibratedCamera::Project(pointAtInfinity, test1);
|
||||
|
|
@ -127,6 +127,7 @@ static Point2 project2(const CalibratedCamera& camera, const Point3& point) {
|
|||
return camera.project(point);
|
||||
}
|
||||
|
||||
|
||||
TEST( CalibratedCamera, Dproject_point_pose)
|
||||
{
|
||||
Matrix Dpose, Dpoint;
|
||||
|
|
@ -173,7 +174,7 @@ TEST( CalibratedCamera, Dproject_point_pose_infinity)
|
|||
// Add a test with more arbitrary rotation
|
||||
TEST( CalibratedCamera, Dproject_point_pose2_infinity)
|
||||
{
|
||||
static const Pose3 pose(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
|
||||
static const Pose3 pose(Rot3::Ypr(-M_PI/4.0, M_PI + M_PI/9.5, M_PI/8.0), Point3(0, 0, -10));
|
||||
static const CalibratedCamera camera(pose);
|
||||
Matrix Dpose, Dpoint;
|
||||
camera.project2(pointAtInfinity, Dpose, Dpoint);
|
||||
|
|
@ -183,7 +184,48 @@ TEST( CalibratedCamera, Dproject_point_pose2_infinity)
|
|||
CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
|
||||
}
|
||||
|
||||
static Point3 BackprojectFromCamera(const CalibratedCamera& camera, const Point2& point,
|
||||
const double& depth) {
|
||||
return camera.BackprojectFromCamera(point, depth);
|
||||
}
|
||||
TEST( CalibratedCamera, DBackprojectFromCamera)
|
||||
{
|
||||
static const Pose3 pose(Rot3::Ypr(-M_PI/4.0, M_PI + M_PI/9.5, M_PI/8.0), Point3(0, 0, -10));
|
||||
static const CalibratedCamera camera(pose);
|
||||
static const double depth = 5.4;
|
||||
static const Point2 point(10.1, 50.3);
|
||||
Matrix Dpoint, Ddepth;
|
||||
camera.BackprojectFromCamera(point, depth, Dpoint, Ddepth);
|
||||
Matrix numerical_point = numericalDerivative32(BackprojectFromCamera, camera, point, depth);
|
||||
Matrix numerical_depth = numericalDerivative33(BackprojectFromCamera, camera, point, depth);
|
||||
CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
|
||||
CHECK(assert_equal(numerical_depth, Ddepth, 1e-7));
|
||||
}
|
||||
|
||||
|
||||
static Point3 backproject(const Pose3& pose, const Point2& point, const double& depth) {
|
||||
return CalibratedCamera(pose).backproject(point, depth);
|
||||
}
|
||||
TEST( PinholePose, Dbackproject)
|
||||
{
|
||||
Matrix36 Dpose;
|
||||
Matrix31 Ddepth;
|
||||
Matrix32 Dpoint;
|
||||
const Point2 point(-100, 100);
|
||||
const double depth(10);
|
||||
static const Pose3 pose(Rot3::Ypr(-M_PI/4.0, M_PI + M_PI/9.5, M_PI/8.0), Point3(0, 0, -10));
|
||||
static const CalibratedCamera camera(pose);
|
||||
camera.backproject(point, depth, Dpose, Dpoint, Ddepth);
|
||||
Matrix expectedDpose = numericalDerivative31(backproject, pose, point, depth);
|
||||
Matrix expectedDpoint = numericalDerivative32(backproject, pose, point, depth);
|
||||
Matrix expectedDdepth = numericalDerivative33(backproject, pose,point, depth);
|
||||
|
||||
EXPECT(assert_equal(expectedDpose, Dpose, 1e-7));
|
||||
EXPECT(assert_equal(expectedDpoint, Dpoint, 1e-7));
|
||||
EXPECT(assert_equal(expectedDdepth, Ddepth, 1e-7));
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
|
|||
|
|
@ -191,7 +191,7 @@ static Point2 project(const Pose3& pose, const Unit3& pointAtInfinity,
|
|||
/* ************************************************************************* */
|
||||
TEST( PinholePose, DprojectAtInfinity2)
|
||||
{
|
||||
Unit3 pointAtInfinity(0,0,1000);
|
||||
Unit3 pointAtInfinity(0,0,-1000);
|
||||
Matrix Dpose, Dpoint;
|
||||
Point2 result = camera.project2(pointAtInfinity, Dpose, Dpoint);
|
||||
Matrix expectedDcamera = numericalDerivative31(project, pose, pointAtInfinity, K);
|
||||
|
|
@ -201,6 +201,32 @@ TEST( PinholePose, DprojectAtInfinity2)
|
|||
EXPECT(assert_equal(expectedDpoint, Dpoint, 1e-7));
|
||||
}
|
||||
|
||||
|
||||
static Point3 backproject(const Pose3& pose, const Cal3_S2& cal,
|
||||
const Point2& p, const double& depth) {
|
||||
return Camera(pose, cal.vector()).backproject(p, depth);
|
||||
}
|
||||
|
||||
TEST( PinholePose, Dbackproject)
|
||||
{
|
||||
Matrix36 Dpose;
|
||||
Matrix31 Ddepth;
|
||||
Matrix32 Dpoint;
|
||||
Matrix Dcal;
|
||||
const Point2 point(-100, 100);
|
||||
const double depth(10);
|
||||
camera.backproject(point, depth, Dpose, Dpoint, Ddepth, Dcal);
|
||||
Matrix expectedDpose = numericalDerivative41(backproject, pose, *K, point, depth);
|
||||
Matrix expectedDcal = numericalDerivative42(backproject, pose, *K, point, depth);
|
||||
Matrix expectedDpoint = numericalDerivative43(backproject, pose, *K, point, depth);
|
||||
Matrix expectedDdepth = numericalDerivative44(backproject, pose, *K, point, depth);
|
||||
|
||||
EXPECT(assert_equal(expectedDpose, Dpose, 1e-7));
|
||||
EXPECT(assert_equal(expectedDcal, Dcal, 1e-7));
|
||||
EXPECT(assert_equal(expectedDpoint, Dpoint, 1e-7));
|
||||
EXPECT(assert_equal(expectedDdepth, Ddepth, 1e-7));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
static double range0(const Camera& camera, const Point3& point) {
|
||||
return camera.range(point);
|
||||
|
|
|
|||
|
|
@ -22,43 +22,48 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESTREE>
|
||||
void ISAM<BAYESTREE>::update_internal(const FactorGraphType& newFactors, Cliques& orphans, const Eliminate& function)
|
||||
{
|
||||
// Remove the contaminated part of the Bayes tree
|
||||
BayesNetType bn;
|
||||
if (!this->empty()) {
|
||||
const KeySet newFactorKeys = newFactors.keys();
|
||||
this->removeTop(std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end()), bn, orphans);
|
||||
}
|
||||
|
||||
// Add the removed top and the new factors
|
||||
FactorGraphType factors;
|
||||
factors += bn;
|
||||
factors += newFactors;
|
||||
|
||||
// Add the orphaned subtrees
|
||||
for(const sharedClique& orphan: orphans)
|
||||
factors += boost::make_shared<BayesTreeOrphanWrapper<Clique> >(orphan);
|
||||
|
||||
// eliminate into a Bayes net
|
||||
const VariableIndex varIndex(factors);
|
||||
const KeySet newFactorKeys = newFactors.keys();
|
||||
const Ordering constrainedOrdering =
|
||||
Ordering::ColamdConstrainedLast(varIndex, std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end()));
|
||||
Base bayesTree = *factors.eliminateMultifrontal(constrainedOrdering, function, varIndex);
|
||||
this->roots_.insert(this->roots_.end(), bayesTree.roots().begin(), bayesTree.roots().end());
|
||||
this->nodes_.insert(bayesTree.nodes().begin(), bayesTree.nodes().end());
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESTREE>
|
||||
void ISAM<BAYESTREE>::update_internal(const FactorGraphType& newFactors,
|
||||
Cliques& orphans, const Eliminate& function) {
|
||||
// Remove the contaminated part of the Bayes tree
|
||||
BayesNetType bn;
|
||||
const KeySet newFactorKeys = newFactors.keys();
|
||||
if (!this->empty()) {
|
||||
std::vector<Key> keyVector(newFactorKeys.begin(), newFactorKeys.end());
|
||||
this->removeTop(keyVector, bn, orphans);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESTREE>
|
||||
void ISAM<BAYESTREE>::update(const FactorGraphType& newFactors, const Eliminate& function)
|
||||
{
|
||||
Cliques orphans;
|
||||
this->update_internal(newFactors, orphans, function);
|
||||
}
|
||||
// Add the removed top and the new factors
|
||||
FactorGraphType factors;
|
||||
factors += bn;
|
||||
factors += newFactors;
|
||||
|
||||
// Add the orphaned subtrees
|
||||
for (const sharedClique& orphan : orphans)
|
||||
factors += boost::make_shared<BayesTreeOrphanWrapper<Clique> >(orphan);
|
||||
|
||||
// Get an ordering where the new keys are eliminated last
|
||||
const VariableIndex index(factors);
|
||||
const Ordering ordering = Ordering::ColamdConstrainedLast(index,
|
||||
std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end()));
|
||||
|
||||
// eliminate all factors (top, added, orphans) into a new Bayes tree
|
||||
auto bayesTree = factors.eliminateMultifrontal(ordering, function, index);
|
||||
|
||||
// Re-add into Bayes tree data structures
|
||||
this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(),
|
||||
bayesTree->roots().end());
|
||||
this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESTREE>
|
||||
void ISAM<BAYESTREE>::update(const FactorGraphType& newFactors,
|
||||
const Eliminate& function) {
|
||||
Cliques orphans;
|
||||
this->update_internal(newFactors, orphans, function);
|
||||
}
|
||||
|
||||
}
|
||||
/// namespace gtsam
|
||||
|
|
|
|||
|
|
@ -11,7 +11,7 @@
|
|||
/**
|
||||
* @file dataset.cpp
|
||||
* @date Jan 22, 2010
|
||||
* @author nikai, Luca Carlone
|
||||
* @author Kai Ni, Luca Carlone, Frank Dellaert
|
||||
* @brief utility functions for loading datasets
|
||||
*/
|
||||
|
||||
|
|
@ -56,8 +56,10 @@ namespace gtsam {
|
|||
string findExampleDataFile(const string& name) {
|
||||
// Search source tree and installed location
|
||||
vector<string> rootsToSearch;
|
||||
rootsToSearch.push_back(GTSAM_SOURCE_TREE_DATASET_DIR); // Defined by CMake, see gtsam/gtsam/CMakeLists.txt
|
||||
rootsToSearch.push_back(GTSAM_INSTALLED_DATASET_DIR); // Defined by CMake, see gtsam/gtsam/CMakeLists.txt
|
||||
|
||||
// Constants below are defined by CMake, see gtsam/gtsam/CMakeLists.txt
|
||||
rootsToSearch.push_back(GTSAM_SOURCE_TREE_DATASET_DIR);
|
||||
rootsToSearch.push_back(GTSAM_INSTALLED_DATASET_DIR);
|
||||
|
||||
// Search for filename as given, and with .graph and .txt extensions
|
||||
vector<string> namesToSearch;
|
||||
|
|
@ -75,12 +77,11 @@ string findExampleDataFile(const string& name) {
|
|||
}
|
||||
|
||||
// If we did not return already, then we did not find the file
|
||||
throw
|
||||
invalid_argument(
|
||||
"gtsam::findExampleDataFile could not find a matching file in\n"
|
||||
GTSAM_SOURCE_TREE_DATASET_DIR " or\n"
|
||||
GTSAM_INSTALLED_DATASET_DIR " named\n" +
|
||||
name + ", " + name + ".graph, or " + name + ".txt");
|
||||
throw invalid_argument(
|
||||
"gtsam::findExampleDataFile could not find a matching file in\n"
|
||||
GTSAM_SOURCE_TREE_DATASET_DIR " or\n"
|
||||
GTSAM_INSTALLED_DATASET_DIR " named\n" + name + ", " + name
|
||||
+ ".graph, or " + name + ".txt");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -98,6 +99,7 @@ string createRewrittenFileName(const string& name) {
|
|||
|
||||
return newpath.string();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
#endif
|
||||
|
||||
|
|
@ -116,23 +118,20 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart,
|
|||
double v1, v2, v3, v4, v5, v6;
|
||||
is >> v1 >> v2 >> v3 >> v4 >> v5 >> v6;
|
||||
|
||||
if (noiseFormat == NoiseFormatAUTO)
|
||||
{
|
||||
// Try to guess covariance matrix layout
|
||||
if(v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0 && v6 == 0.0)
|
||||
{
|
||||
// NoiseFormatGRAPH
|
||||
noiseFormat = NoiseFormatGRAPH;
|
||||
}
|
||||
else if(v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0 && v6 != 0.0)
|
||||
{
|
||||
// NoiseFormatCOV
|
||||
noiseFormat = NoiseFormatCOV;
|
||||
}
|
||||
else
|
||||
{
|
||||
throw std::invalid_argument("load2D: unrecognized covariance matrix format in dataset file. Please specify the noise format.");
|
||||
}
|
||||
if (noiseFormat == NoiseFormatAUTO) {
|
||||
// Try to guess covariance matrix layout
|
||||
if (v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0
|
||||
&& v6 == 0.0) {
|
||||
// NoiseFormatGRAPH
|
||||
noiseFormat = NoiseFormatGRAPH;
|
||||
} else if (v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0
|
||||
&& v6 != 0.0) {
|
||||
// NoiseFormatCOV
|
||||
noiseFormat = NoiseFormatCOV;
|
||||
} else {
|
||||
throw std::invalid_argument(
|
||||
"load2D: unrecognized covariance matrix format in dataset file. Please specify the noise format.");
|
||||
}
|
||||
}
|
||||
|
||||
// Read matrix and check that diagonal entries are non-zero
|
||||
|
|
@ -195,6 +194,32 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart,
|
|||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
boost::optional<IndexedPose> parseVertex(istream& is, const string& tag) {
|
||||
if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) {
|
||||
Key id;
|
||||
double x, y, yaw;
|
||||
is >> id >> x >> y >> yaw;
|
||||
return IndexedPose(id, Pose2(x, y, yaw));
|
||||
} else {
|
||||
return boost::none;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
boost::optional<IndexedEdge> parseEdge(istream& is, const string& tag) {
|
||||
if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2")
|
||||
|| (tag == "ODOMETRY")) {
|
||||
|
||||
Key id1, id2;
|
||||
double x, y, yaw;
|
||||
is >> id1 >> id2 >> x >> y >> yaw;
|
||||
return IndexedEdge(pair<Key, Key>(id1, id2), Pose2(x, y, yaw));
|
||||
} else {
|
||||
return boost::none;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
|
||||
bool addNoise, bool smart, NoiseFormat noiseFormat,
|
||||
|
|
@ -214,16 +239,15 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
|
|||
if (!(is >> tag))
|
||||
break;
|
||||
|
||||
if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) {
|
||||
Key id;
|
||||
double x, y, yaw;
|
||||
is >> id >> x >> y >> yaw;
|
||||
const auto indexed_pose = parseVertex(is, tag);
|
||||
if (indexed_pose) {
|
||||
Key id = indexed_pose->first;
|
||||
|
||||
// optional filter
|
||||
if (maxID && id >= maxID)
|
||||
continue;
|
||||
|
||||
initial->insert(id, Pose2(x, y, yaw));
|
||||
initial->insert(id, indexed_pose->second);
|
||||
}
|
||||
is.ignore(LINESIZE, '\n');
|
||||
}
|
||||
|
|
@ -251,13 +275,10 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
|
|||
if (!(is >> tag))
|
||||
break;
|
||||
|
||||
if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2")
|
||||
|| (tag == "ODOMETRY")) {
|
||||
|
||||
// Read transform
|
||||
double x, y, yaw;
|
||||
is >> id1 >> id2 >> x >> y >> yaw;
|
||||
Pose2 l1Xl2(x, y, yaw);
|
||||
auto between_pose = parseEdge(is, tag);
|
||||
if (between_pose) {
|
||||
std::tie(id1, id2) = between_pose->first;
|
||||
Pose2& l1Xl2 = between_pose->second;
|
||||
|
||||
// read noise model
|
||||
SharedNoiseModel modelInFile = readNoiseModel(is, smart, noiseFormat,
|
||||
|
|
|
|||
|
|
@ -33,6 +33,7 @@
|
|||
#include <string>
|
||||
#include <utility> // for pair
|
||||
#include <vector>
|
||||
#include <iosfwd>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -71,6 +72,26 @@ enum KernelFunctionType {
|
|||
KernelFunctionTypeNONE, KernelFunctionTypeHUBER, KernelFunctionTypeTUKEY
|
||||
};
|
||||
|
||||
/// Return type for auxiliary functions
|
||||
typedef std::pair<Key, Pose2> IndexedPose;
|
||||
typedef std::pair<std::pair<Key, Key>, Pose2> IndexedEdge;
|
||||
|
||||
/**
|
||||
* Parse TORO/G2O vertex "id x y yaw"
|
||||
* @param is input stream
|
||||
* @param tag string parsed from input stream, will only parse if vertex type
|
||||
*/
|
||||
GTSAM_EXPORT boost::optional<IndexedPose> parseVertex(std::istream& is,
|
||||
const std::string& tag);
|
||||
|
||||
/**
|
||||
* Parse TORO/G2O edge "id1 id2 x y yaw"
|
||||
* @param is input stream
|
||||
* @param tag string parsed from input stream, will only parse if edge type
|
||||
*/
|
||||
GTSAM_EXPORT boost::optional<IndexedEdge> parseEdge(std::istream& is,
|
||||
const std::string& tag);
|
||||
|
||||
/// Return type for load functions
|
||||
typedef std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> GraphAndValues;
|
||||
|
||||
|
|
|
|||
|
|
@ -26,6 +26,9 @@
|
|||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
|
||||
using namespace gtsam::symbol_shorthand;
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
|
@ -39,6 +42,37 @@ TEST(dataSet, findExampleDataFile) {
|
|||
EXPECT(assert_equal(expected_end, actual_end));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( dataSet, parseVertex)
|
||||
{
|
||||
const string str = "VERTEX2 1 2.000000 3.000000 4.000000";
|
||||
istringstream is(str);
|
||||
string tag;
|
||||
EXPECT(is >> tag);
|
||||
const auto actual = parseVertex(is, tag);
|
||||
EXPECT(actual);
|
||||
if (actual) {
|
||||
EXPECT_LONGS_EQUAL(1, actual->first);
|
||||
EXPECT(assert_equal(Pose2(2, 3, 4), actual->second));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( dataSet, parseEdge)
|
||||
{
|
||||
const string str = "EDGE2 0 1 2.000000 3.000000 4.000000";
|
||||
istringstream is(str);
|
||||
string tag;
|
||||
EXPECT(is >> tag);
|
||||
const auto actual = parseEdge(is, tag);
|
||||
EXPECT(actual);
|
||||
if (actual) {
|
||||
pair<Key, Key> expected(0, 1);
|
||||
EXPECT(expected == actual->first);
|
||||
EXPECT(assert_equal(Pose2(2, 3, 4), actual->second));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( dataSet, load2D)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -36,31 +36,38 @@ struct PinholeBaseKCal3_S2Callback : PinholeBaseKCal3_S2, wrapper<PinholeBaseKCa
|
|||
}
|
||||
};
|
||||
|
||||
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(project_overloads, PinholeBaseKCal3_S2::project, 2, 4)
|
||||
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(project_overloads, PinholeBaseKCal3_S2::project, 1, 4)
|
||||
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(range_overloads, PinholeBaseKCal3_S2::range, 1, 3)
|
||||
|
||||
// Function pointers to desambiguate project() calls
|
||||
Point2 (PinholeBaseKCal3_S2::*project1) (const Point3 &pw) const = &PinholeBaseKCal3_S2::project;
|
||||
Point2 (PinholeBaseKCal3_S2::*project2) (const Point3 &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 3 > Dpoint, OptionalJacobian< 2, FixedDimension<Cal3_S2>::value > Dcal) const = &PinholeBaseKCal3_S2::project;
|
||||
Point2 (PinholeBaseKCal3_S2::*project3) (const Unit3 &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 2 > Dpoint, OptionalJacobian< 2, FixedDimension<Cal3_S2>::value > Dcal) const = &PinholeBaseKCal3_S2::project;
|
||||
// Function pointers to disambiguate project() calls
|
||||
Point2 (PinholeBaseKCal3_S2::*project1) (const Point3 &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 3 > Dpoint, OptionalJacobian< 2, FixedDimension<Cal3_S2>::value > Dcal) const = &PinholeBaseKCal3_S2::project;
|
||||
Point2 (PinholeBaseKCal3_S2::*project2) (const Unit3 &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 2 > Dpoint, OptionalJacobian< 2, FixedDimension<Cal3_S2>::value > Dcal) const = &PinholeBaseKCal3_S2::project;
|
||||
|
||||
// function pointers to desambiguate range() calls
|
||||
// function pointers to disambiguate range() calls
|
||||
double (PinholeBaseKCal3_S2::*range1) (const Point3 &point, OptionalJacobian< 1, 6 > Dcamera, OptionalJacobian< 1, 3 > Dpoint) const = &PinholeBaseKCal3_S2::range;
|
||||
double (PinholeBaseKCal3_S2::*range2) (const Pose3 &pose, OptionalJacobian< 1, 6 > Dcamera, OptionalJacobian< 1, 6 > Dpose) const = &PinholeBaseKCal3_S2::range;
|
||||
double (PinholeBaseKCal3_S2::*range3) (const CalibratedCamera &camera, OptionalJacobian< 1, 6 > Dcamera, OptionalJacobian< 1, 6 > Dother) const = &PinholeBaseKCal3_S2::range;
|
||||
|
||||
void exportPinholeBaseK(){
|
||||
|
||||
class_<PinholeBaseKCal3_S2Callback, boost::noncopyable>("PinholeBaseKCal3_S2", no_init)
|
||||
.def("calibration", pure_virtual(&PinholeBaseKCal3_S2::calibration), return_value_policy<copy_const_reference>())
|
||||
.def("project", project1)
|
||||
.def("project", project2, project_overloads())
|
||||
.def("project", project3, project_overloads())
|
||||
.def("backproject", &PinholeBaseKCal3_S2::backproject)
|
||||
.def("backproject_point_at_infinity", &PinholeBaseKCal3_S2::backprojectPointAtInfinity)
|
||||
.def("range", range1, range_overloads())
|
||||
.def("range", range2, range_overloads())
|
||||
.def("range", range3, range_overloads())
|
||||
;
|
||||
|
||||
// wrap projectSafe in a function that returns None or a tuple
|
||||
// TODO(frank): find out how to return an ndarray instead
|
||||
object project_safe(const PinholeBaseKCal3_S2& camera, const gtsam::Point3& p) {
|
||||
auto result = camera.projectSafe(p);
|
||||
if (result.second)
|
||||
return make_tuple(result.first.x(), result.first.y());
|
||||
else
|
||||
return object();
|
||||
}
|
||||
|
||||
void exportPinholeBaseK() {
|
||||
class_<PinholeBaseKCal3_S2Callback, boost::noncopyable>("PinholeBaseKCal3_S2", no_init)
|
||||
.def("calibration", pure_virtual(&PinholeBaseKCal3_S2::calibration),
|
||||
return_value_policy<copy_const_reference>())
|
||||
.def("project_safe", make_function(project_safe))
|
||||
.def("project", project1, project_overloads())
|
||||
.def("project", project2, project_overloads())
|
||||
.def("backproject", &PinholeBaseKCal3_S2::backproject)
|
||||
.def("backproject_point_at_infinity", &PinholeBaseKCal3_S2::backprojectPointAtInfinity)
|
||||
.def("range", range1, range_overloads())
|
||||
.def("range", range2, range_overloads())
|
||||
.def("range", range3, range_overloads());
|
||||
}
|
||||
|
|
@ -7,6 +7,7 @@
|
|||
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/sam/BearingRangeFactor.h>
|
||||
#include <gtsam/slam/dataset.h>
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/nonlinear/NonlinearEquality.h>
|
||||
#include <gtsam/nonlinear/NonlinearISAM.h>
|
||||
|
|
@ -16,7 +17,10 @@
|
|||
#include <gtsam/linear/Sampler.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
const double tol=1e-5;
|
||||
|
|
@ -228,6 +232,93 @@ TEST(testNonlinearISAM, markov_chain_with_reconnect ) {
|
|||
EXPECT(assert_equal(expected.at<Point2>(lm3), actualQR.at<Point2>(lm3), tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(testNonlinearISAM, loop_closures ) {
|
||||
int relinearizeInterval = 100;
|
||||
NonlinearISAM isam(relinearizeInterval);
|
||||
|
||||
// Create a Factor Graph and Values to hold the new data
|
||||
NonlinearFactorGraph graph;
|
||||
Values initialEstimate;
|
||||
|
||||
vector<string> lines;
|
||||
lines.emplace_back("VERTEX2 0 0.000000 0.000000 0.000000");
|
||||
lines.emplace_back("EDGE2 1 0 1.030390 0.011350 -0.012958");
|
||||
lines.emplace_back("VERTEX2 1 1.030390 0.011350 -0.012958");
|
||||
lines.emplace_back("EDGE2 2 1 1.013900 -0.058639 -0.013225");
|
||||
lines.emplace_back("VERTEX2 2 2.043445 -0.060422 -0.026183");
|
||||
lines.emplace_back("EDGE2 3 2 1.027650 -0.007456 0.004833");
|
||||
lines.emplace_back("VERTEX2 3 3.070548 -0.094779 -0.021350");
|
||||
lines.emplace_back("EDGE2 4 3 -0.012016 1.004360 1.566790");
|
||||
lines.emplace_back("VERTEX2 4 3.079976 0.909609 1.545440");
|
||||
lines.emplace_back("EDGE2 5 4 1.016030 0.014565 -0.016304");
|
||||
lines.emplace_back("VERTEX2 5 3.091176 1.925681 1.529136");
|
||||
lines.emplace_back("EDGE2 6 5 1.023890 0.006808 0.010981");
|
||||
lines.emplace_back("VERTEX2 6 3.127018 2.948966 1.540117");
|
||||
lines.emplace_back("EDGE2 7 6 0.957734 0.003159 0.010901");
|
||||
lines.emplace_back("VERTEX2 7 3.153237 3.906347 1.551018");
|
||||
lines.emplace_back("EDGE2 8 7 -1.023820 -0.013668 -3.093240");
|
||||
lines.emplace_back("VERTEX2 8 3.146655 2.882457 -1.542222");
|
||||
lines.emplace_back("EDGE2 9 8 1.023440 0.013984 -0.007802");
|
||||
lines.emplace_back("EDGE2 9 5 0.033943 0.032439 -3.127400");
|
||||
lines.emplace_back("VERTEX2 9 3.189873 1.859834 -1.550024");
|
||||
lines.emplace_back("EDGE2 10 9 1.003350 0.022250 0.023491");
|
||||
lines.emplace_back("EDGE2 10 3 0.044020 0.988477 -1.563530");
|
||||
lines.emplace_back("VERTEX2 10 3.232959 0.857162 -1.526533");
|
||||
lines.emplace_back("EDGE2 11 10 0.977245 0.019042 -0.028623");
|
||||
lines.emplace_back("VERTEX2 11 3.295225 -0.118283 -1.555156");
|
||||
lines.emplace_back("EDGE2 12 11 -0.996880 -0.025512 -3.126915");
|
||||
lines.emplace_back("VERTEX2 12 3.254125 0.878076 1.601114");
|
||||
lines.emplace_back("EDGE2 13 12 0.990646 0.018396 -0.016519");
|
||||
lines.emplace_back("VERTEX2 13 3.205708 1.867709 1.584594");
|
||||
lines.emplace_back("EDGE2 14 13 0.945873 0.008893 -0.002602");
|
||||
lines.emplace_back("EDGE2 14 8 0.015808 0.021059 3.128310");
|
||||
lines.emplace_back("VERTEX2 14 3.183765 2.813370 1.581993");
|
||||
lines.emplace_back("EDGE2 15 14 1.000010 0.006428 0.028234");
|
||||
lines.emplace_back("EDGE2 15 7 -0.014728 -0.001595 -0.019579");
|
||||
lines.emplace_back("VERTEX2 15 3.166141 3.813245 1.610227");
|
||||
|
||||
auto model = noiseModel::Diagonal::Sigmas(Vector3(3.0, 3.0, 0.5));
|
||||
|
||||
// Loop over the different poses, adding the observations to iSAM incrementally
|
||||
for (const string& str : lines) {
|
||||
// scan the tag
|
||||
string tag;
|
||||
istringstream is(str);
|
||||
if (!(is >> tag))
|
||||
break;
|
||||
|
||||
// Check if vertex
|
||||
const auto indexedPose = parseVertex(is, tag);
|
||||
if (indexedPose) {
|
||||
Key id = indexedPose->first;
|
||||
initialEstimate.insert(Symbol('x', id), indexedPose->second);
|
||||
if (id == 0) {
|
||||
noiseModel::Diagonal::shared_ptr priorNoise =
|
||||
noiseModel::Diagonal::Sigmas(Vector3(0.001, 0.001, 0.001));
|
||||
graph.emplace_shared<PriorFactor<Pose2> >(Symbol('x', id),
|
||||
Pose2(0, 0, 0), priorNoise);
|
||||
} else {
|
||||
isam.update(graph, initialEstimate);
|
||||
|
||||
// Clear the factor graph and values for the next iteration
|
||||
graph.resize(0);
|
||||
initialEstimate.clear();
|
||||
}
|
||||
}
|
||||
|
||||
// check if edge
|
||||
const auto betweenPose = parseEdge(is, tag);
|
||||
if (betweenPose) {
|
||||
Key id1, id2;
|
||||
tie(id1, id2) = betweenPose->first;
|
||||
graph.emplace_shared<BetweenFactor<Pose2> >(Symbol('x', id2),
|
||||
Symbol('x', id1), betweenPose->second, model);
|
||||
}
|
||||
}
|
||||
EXPECT_LONGS_EQUAL(16, isam.estimate().size())
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue