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

done with geometry

release/4.3a0
Natesh Srinivasan 2014-12-03 16:34:58 -05:00
parent aad0b2876b
commit 0f95890215
6 changed files with 18 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
gtsam/base/Matrix.h
View File

@ -163,46 +163,6 @@ static const Eigen::DenseBase<Matrix41>::ConstantReturnType O_4x1 = Matrix41::On
static const Eigen::DenseBase<Matrix51>::ConstantReturnType O_5x1 = Matrix51::Ones(); static const Eigen::DenseBase<Matrix51>::ConstantReturnType O_5x1 = Matrix51::Ones();
static const Eigen::DenseBase<Matrix61>::ConstantReturnType O_6x1 = Matrix61::Ones(); static const Eigen::DenseBase<Matrix61>::ConstantReturnType O_6x1 = Matrix61::Ones();
// Negative Ones
static const Eigen::DenseBase<Matrix1>::ConstantReturnType _O_1x1 = Matrix1::Constant(-1);
static const Eigen::DenseBase<Matrix2>::ConstantReturnType _O_2x2 = Matrix2::Constant(-1);
static const Eigen::DenseBase<Matrix3>::ConstantReturnType _O_3x3 = Matrix3::Constant(-1);
static const Eigen::DenseBase<Matrix4>::ConstantReturnType _O_4x4 = Matrix4::Constant(-1);
static const Eigen::DenseBase<Matrix5>::ConstantReturnType _O_5x5 = Matrix5::Constant(-1);
static const Eigen::DenseBase<Matrix6>::ConstantReturnType _O_6x6 = Matrix6::Constant(-1);
static const Eigen::DenseBase<Matrix12>::ConstantReturnType _O_1x2 = Matrix12::Constant(-1);
static const Eigen::DenseBase<Matrix13>::ConstantReturnType _O_1x3 = Matrix13::Constant(-1);
static const Eigen::DenseBase<Matrix14>::ConstantReturnType _O_1x4 = Matrix14::Constant(-1);
static const Eigen::DenseBase<Matrix15>::ConstantReturnType _O_1x5 = Matrix15::Constant(-1);
static const Eigen::DenseBase<Matrix16>::ConstantReturnType _O_1x6 = Matrix16::Constant(-1);
static const Eigen::DenseBase<Matrix23>::ConstantReturnType _O_2x3 = Matrix23::Constant(-1);
static const Eigen::DenseBase<Matrix24>::ConstantReturnType _O_2x4 = Matrix24::Constant(-1);
static const Eigen::DenseBase<Matrix25>::ConstantReturnType _O_2x5 = Matrix25::Constant(-1);
static const Eigen::DenseBase<Matrix26>::ConstantReturnType _O_2x6 = Matrix26::Constant(-1);
static const Eigen::DenseBase<Matrix27>::ConstantReturnType _O_2x7 = Matrix27::Constant(-1);
static const Eigen::DenseBase<Matrix28>::ConstantReturnType _O_2x8 = Matrix28::Constant(-1);
static const Eigen::DenseBase<Matrix29>::ConstantReturnType _O_2x9 = Matrix29::Constant(-1);
static const Eigen::DenseBase<Matrix32>::ConstantReturnType _O_3x2 = Matrix32::Constant(-1);
static const Eigen::DenseBase<Matrix34>::ConstantReturnType _O_3x4 = Matrix34::Constant(-1);
static const Eigen::DenseBase<Matrix35>::ConstantReturnType _O_3x5 = Matrix35::Constant(-1);
static const Eigen::DenseBase<Matrix36>::ConstantReturnType _O_3x6 = Matrix36::Constant(-1);
static const Eigen::DenseBase<Matrix37>::ConstantReturnType _O_3x7 = Matrix37::Constant(-1);
static const Eigen::DenseBase<Matrix38>::ConstantReturnType _O_3x8 = Matrix38::Constant(-1);
static const Eigen::DenseBase<Matrix39>::ConstantReturnType _O_3x9 = Matrix39::Constant(-1);
static const Eigen::DenseBase<Matrix21>::ConstantReturnType _O_2x1 = Matrix21::Constant(-1);
static const Eigen::DenseBase<Matrix31>::ConstantReturnType _O_3x1 = Matrix31::Constant(-1);
static const Eigen::DenseBase<Matrix41>::ConstantReturnType _O_4x1 = Matrix41::Constant(-1);
static const Eigen::DenseBase<Matrix51>::ConstantReturnType _O_5x1 = Matrix51::Constant(-1);
static const Eigen::DenseBase<Matrix61>::ConstantReturnType _O_6x1 = Matrix61::Constant(-1);
// Matlab-like syntax // Matlab-like syntax
/** /**

23
gtsam/geometry/StereoCamera.cpp
View File

@ -30,7 +30,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
StereoPoint2 StereoCamera::project(const Point3& point, StereoPoint2 StereoCamera::project(const Point3& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2) const {
#ifdef STEREOCAMERA_CHAIN_RULE #ifdef STEREOCAMERA_CHAIN_RULE
const Point3 q = leftCamPose_.transform_to(point, H1, H2); const Point3 q = leftCamPose_.transform_to(point, H1, H2);
@ -57,26 +57,23 @@ namespace gtsam {
if (H1 || H2) { if (H1 || H2) {
#ifdef STEREOCAMERA_CHAIN_RULE #ifdef STEREOCAMERA_CHAIN_RULE
// just implement chain rule // just implement chain rule
Matrix D_project_point = Dproject_to_stereo_camera1(q); // 3x3 Jacobian Matrix3 D_project_point = Dproject_to_stereo_camera1(q); // 3x3 Jacobian
if (H1) *H1 = D_project_point*(*H1); if (H1) *H1 = D_project_point*(*H1);
if (H2) *H2 = D_project_point*(*H2); if (H2) *H2 = D_project_point*(*H2);
#else #else
// optimized version, see StereoCamera.nb // optimized version, see StereoCamera.nb
if (H1) { if (H1) {
const double v1 = v/fy, v2 = fx*v1, dx=d*x; const double v1 = v/fy, v2 = fx*v1, dx=d*x;
*H1 = (Matrix(3, 6) << *H1 << uL*v1, -fx-dx*uL, v2, -dfx, 0.0, d*uL,
uL*v1, -fx-dx*uL, v2, -dfx, 0.0, d*uL,
uR*v1, -fx-dx*uR, v2, -dfx, 0.0, d*uR, uR*v1, -fx-dx*uR, v2, -dfx, 0.0, d*uR,
fy + v*v1, -dx*v , -x*dfy, 0.0, -dfy, d*v fy + v*v1, -dx*v , -x*dfy, 0.0, -dfy, d*v;
).finished();
} }
if (H2) { if (H2) {
const Matrix R(leftCamPose_.rotation().matrix()); const Matrix3 R(leftCamPose_.rotation().matrix());
*H2 = d * (Matrix(3, 3) << *H2 << fx*R(0, 0) - R(0, 2)*uL, fx*R(1, 0) - R(1, 2)*uL, fx*R(2, 0) - R(2, 2)*uL,
fx*R(0, 0) - R(0, 2)*uL, fx*R(1, 0) - R(1, 2)*uL, fx*R(2, 0) - R(2, 2)*uL, fx*R(0, 0) - R(0, 2)*uR, fx*R(1, 0) - R(1, 2)*uR, fx*R(2, 0) - R(2, 2)*uR,
fx*R(0, 0) - R(0, 2)*uR, fx*R(1, 0) - R(1, 2)*uR, fx*R(2, 0) - R(2, 2)*uR, fy*R(0, 1) - R(0, 2)*v , fy*R(1, 1) - R(1, 2)*v , fy*R(2, 1) - R(2, 2)*v;
fy*R(0, 1) - R(0, 2)*v , fy*R(1, 1) - R(1, 2)*v , fy*R(2, 1) - R(2, 2)*v *H2 << d * (*H2);
).finished();
} }
#endif #endif
} }
@ -86,7 +83,7 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const { Matrix3 StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const {
double d = 1.0 / P.z(), d2 = d*d; double d = 1.0 / P.z(), d2 = d*d;
const Cal3_S2Stereo& K = *K_; const Cal3_S2Stereo& K = *K_;
double f_x = K.fx(), f_y = K.fy(), b = K.baseline(); double f_x = K.fx(), f_y = K.fy(), b = K.baseline();

10
gtsam/geometry/StereoCamera.h
View File

@ -95,8 +95,8 @@ public:
} }
Pose3 between(const StereoCamera &camera, Pose3 between(const StereoCamera &camera,
boost::optional<Matrix&> H1=boost::none, OptionalJacobian<6,6> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const { OptionalJacobian<6,6> H2=boost::none) const {
return leftCamPose_.between(camera.pose(), H1, H2); return leftCamPose_.between(camera.pose(), H1, H2);
} }
@ -119,8 +119,8 @@ public:
* @param H3 IGNORED (for calibration) * @param H3 IGNORED (for calibration)
*/ */
StereoPoint2 project(const Point3& point, StereoPoint2 project(const Point3& point,
boost::optional<Matrix&> H1 = boost::none, OptionalJacobian<3, 6> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const; OptionalJacobian<3, 3> H2 = boost::none) const;
/** /**
* *
@ -138,7 +138,7 @@ public:
private: private:
/** utility functions */ /** utility functions */
Matrix Dproject_to_stereo_camera1(const Point3& P) const; Matrix3 Dproject_to_stereo_camera1(const Point3& P) const;
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class Archive>

4
gtsam/geometry/TriangulationFactor.h
View File

@ -112,14 +112,14 @@ public:
} }
/// Evaluate error h(x)-z and optionally derivatives /// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Point3& point, boost::optional<Matrix&> H2 = Vector evaluateError(const Point3& point, OptionalJacobian<2,3> H2 =
boost::none) const { boost::none) const {
try { try {
Point2 error(camera_.project(point, boost::none, H2, boost::none) - measured_); Point2 error(camera_.project(point, boost::none, H2, boost::none) - measured_);
return error.vector(); return error.vector();
} catch (CheiralityException& e) { } catch (CheiralityException& e) {
if (H2) if (H2)
*H2 = zeros(2, 3); *H2 << Z_2x3;
if (verboseCheirality_) if (verboseCheirality_)
std::cout << e.what() << ": Landmark " std::cout << e.what() << ": Landmark "
<< DefaultKeyFormatter(this->key()) << " moved behind camera" << DefaultKeyFormatter(this->key()) << " moved behind camera"

1
gtsam/geometry/Unit3.h
View File

@ -147,7 +147,6 @@ private:
/// @name Advanced Interface /// @name Advanced Interface
/// @{ /// @{
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>

2
gtsam/geometry/triangulation.h
View File

@ -53,7 +53,7 @@ public:
* @return Triangulated Point3 * @return Triangulated Point3
*/ */
GTSAM_EXPORT Point3 triangulateDLT( GTSAM_EXPORT Point3 triangulateDLT(
const std::vector<Matrix>& projection_matrices, const std::vector<Matrix>& projection_matrices, // TODO: Use the fact that projection matrices sizes are known at compile time
const std::vector<Point2>& measurements, double rank_tol); const std::vector<Point2>& measurements, double rank_tol);
/// ///