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

Fixed range

release/4.3a0
dellaert 2014-12-04 09:38:28 +01:00
parent 52c4771bcb
commit 354de17fd7
2 changed files with 57 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
gtsam/geometry/CalibratedCamera.h
View File

@ -18,9 +18,8 @@
#pragma once #pragma once
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point2.h>
namespace gtsam { namespace gtsam {
@ -88,26 +87,6 @@ public:
return pose_; return pose_;
} }
/// compose the two camera poses: TODO Frank says this might not make sense
inline const CalibratedCamera compose(const CalibratedCamera &c,
OptionalJacobian<6,6> H1=boost::none, OptionalJacobian<6,6> H2 =
boost::none) const {
return CalibratedCamera(pose_.compose(c.pose(), H1, H2));
}
/// between the two camera poses: TODO Frank says this might not make sense
inline const CalibratedCamera between(const CalibratedCamera& c,
OptionalJacobian<6,6> H1 = boost::none, OptionalJacobian<6,6> H2 =
boost::none) const {
return CalibratedCamera(pose_.between(c.pose(), H1, H2));
}
/// invert the camera pose: TODO Frank says this might not make sense
inline const CalibratedCamera inverse(OptionalJacobian<6,6> H1 =
boost::none) const {
return CalibratedCamera(pose_.inverse(H1));
}
/** /**
* Create a level camera at the given 2D pose and height * Create a level camera at the given 2D pose and height
* @param pose2 specifies the location and viewing direction * @param pose2 specifies the location and viewing direction
@ -152,7 +131,8 @@ public:
* @return the intrinsic coordinates of the projected point * @return the intrinsic coordinates of the projected point
*/ */
Point2 project(const Point3& point, Point2 project(const Point3& point,
OptionalJacobian<2,6> Dpose=boost::none, OptionalJacobian<2,3> Dpoint=boost::none) const; OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none) const;
/** /**
* projects a 3-dimensional point in camera coordinates into the * projects a 3-dimensional point in camera coordinates into the
@ -174,8 +154,8 @@ public:
* @param H2 optionally computed Jacobian with respect to the 3D point * @param H2 optionally computed Jacobian with respect to the 3D point
* @return range (double) * @return range (double)
*/ */
double range(const Point3& point, OptionalJacobian<1,6> H1 = boost::none, double range(const Point3& point, OptionalJacobian<1, 6> H1 = boost::none,
OptionalJacobian<1,3> H2 = boost::none) const { OptionalJacobian<1, 3> H2 = boost::none) const {
return pose_.range(point, H1, H2); return pose_.range(point, H1, H2);
} }
@ -186,8 +166,8 @@ public:
* @param H2 optionally computed Jacobian with respect to the 3D point * @param H2 optionally computed Jacobian with respect to the 3D point
* @return range (double) * @return range (double)
*/ */
double range(const Pose3& pose, OptionalJacobian<1,6> H1=boost::none, double range(const Pose3& pose, OptionalJacobian<1, 6> H1 = boost::none,
OptionalJacobian<1,6> H2=boost::none) const { OptionalJacobian<1, 6> H2 = boost::none) const {
return pose_.range(pose, H1, H2); return pose_.range(pose, H1, H2);
} }
@ -198,8 +178,8 @@ public:
* @param H2 optionally computed Jacobian with respect to the 3D point * @param H2 optionally computed Jacobian with respect to the 3D point
* @return range (double) * @return range (double)
*/ */
double range(const CalibratedCamera& camera, OptionalJacobian<1,6> H1 = double range(const CalibratedCamera& camera, OptionalJacobian<1, 6> H1 =
boost::none, OptionalJacobian<1,6> H2 = boost::none) const { boost::none, OptionalJacobian<1, 6> H2 = boost::none) const {
return pose_.range(camera.pose_, H1, H2); return pose_.range(camera.pose_, H1, H2);
} }
@ -223,15 +203,16 @@ private:
namespace traits { namespace traits {
template<> template<>
struct GTSAM_EXPORT is_group<CalibratedCamera> : public boost::true_type{ struct GTSAM_EXPORT is_group<CalibratedCamera> : public boost::true_type {
}; };
template<> template<>
struct GTSAM_EXPORT is_manifold<CalibratedCamera> : public boost::true_type{ struct GTSAM_EXPORT is_manifold<CalibratedCamera> : public boost::true_type {
}; };
template<> template<>
struct GTSAM_EXPORT dimension<CalibratedCamera> : public boost::integral_constant<int, 6>{ struct GTSAM_EXPORT dimension<CalibratedCamera> : public boost::integral_constant<
int, 6> {
}; };
} }

106
gtsam/geometry/PinholeCamera.h
View File

@ -18,16 +18,9 @@
#pragma once #pragma once
#include <cmath>
#include <boost/optional.hpp>
#include <boost/serialization/nvp.hpp>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/CalibratedCamera.h> #include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/Pose2.h>
#include <cmath>
namespace gtsam { namespace gtsam {
@ -42,8 +35,9 @@ private:
Pose3 pose_; Pose3 pose_;
Calibration K_; Calibration K_;
// Get dimension of calibration type at compile time // Get dimensions of calibration type and This at compile time
static const int DimK = traits::dimension<Calibration>::value; static const int DimK = traits::dimension<Calibration>::value, //
DimC = 6 + DimK;
public: public:
@ -172,6 +166,18 @@ public:
/// @name Manifold /// @name Manifold
/// @{ /// @{
/// Manifold dimension
inline size_t dim() const {
return DimC;
}
/// Manifold dimension
inline static size_t Dim() {
return DimC;
}
typedef Eigen::Matrix<double, DimC, 1> VectorK6;
/// move a cameras according to d /// move a cameras according to d
PinholeCamera retract(const Vector& d) const { PinholeCamera retract(const Vector& d) const {
if ((size_t) d.size() == 6) if ((size_t) d.size() == 6)
@ -181,8 +187,6 @@ public:
calibration().retract(d.tail(calibration().dim()))); calibration().retract(d.tail(calibration().dim())));
} }
typedef Eigen::Matrix<double, Dim(), 1> VectorK6;
/// return canonical coordinate /// return canonical coordinate
VectorK6 localCoordinates(const PinholeCamera& T2) const { VectorK6 localCoordinates(const PinholeCamera& T2) const {
VectorK6 d; // TODO: why does d.head<6>() not compile?? VectorK6 d; // TODO: why does d.head<6>() not compile??
@ -191,16 +195,6 @@ public:
return d; return d;
} }
/// Manifold dimension
inline size_t dim() const {
return Dim();
}
/// Manifold dimension
inline static size_t Dim() {
return Dim();
}
/// @} /// @}
/// @name Transformations and measurement functions /// @name Transformations and measurement functions
/// @{ /// @{
@ -315,7 +309,7 @@ public:
*/ */
Point2 project2( Point2 project2(
const Point3& pw, // const Point3& pw, //
OptionalJacobian<2, Dim()> Dcamera = boost::none, OptionalJacobian<2, DimC> Dcamera = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none) const { OptionalJacobian<2, 3> Dpoint = boost::none) const {
const Point3 pc = pose_.transform_to(pw); const Point3 pc = pose_.transform_to(pw);
@ -359,85 +353,73 @@ public:
/** /**
* Calculate range to a landmark * Calculate range to a landmark
* @param point 3D location of landmark * @param point 3D location of landmark
* @param Dpose the optionally computed Jacobian with respect to pose * @param Dcamera the optionally computed Jacobian with respect to pose
* @param Dpoint the optionally computed Jacobian with respect to the landmark * @param Dpoint the optionally computed Jacobian with respect to the landmark
* @return range (double) * @return range (double)
*/ */
double range( double range(
const Point3& point, // const Point3& point, //
OptionalJacobian<1, 6> Dpose = boost::none, OptionalJacobian<1, DimC> Dcamera = boost::none,
OptionalJacobian<1, 3> Dpoint = boost::none) const { OptionalJacobian<1, 3> Dpoint = boost::none) const {
double result = pose_.range(point, Dpose, Dpoint); Matrix16 Dpose;
if (Dpose) { double result = pose_.range(point, Dcamera ? &Dpose : 0, Dpoint);
// Add columns of zeros to Jacobian for calibration if (Dcamera)
Matrix& H1r(*Dpose); *Dcamera << Dpose, Eigen::Matrix<double, 1, DimK>::Zero();
H1r.conservativeResize(Eigen::NoChange, Dim());
H1r.block<1, DimK>(0, 6).setZero();
}
return result; return result;
} }
/** /**
* Calculate range to another pose * Calculate range to another pose
* @param pose Other SO(3) pose * @param pose Other SO(3) pose
* @param Dpose the optionally computed Jacobian with respect to pose * @param Dcamera the optionally computed Jacobian with respect to pose
* @param Dpose2 the optionally computed Jacobian with respect to the other pose * @param Dpose2 the optionally computed Jacobian with respect to the other pose
* @return range (double) * @return range (double)
*/ */
double range( double range(
const Pose3& pose, // const Pose3& pose, //
boost::optional<Matrix&> Dpose = boost::none, OptionalJacobian<1, DimC> Dcamera = boost::none,
boost::optional<Matrix&> Dpose2 = boost::none) const { OptionalJacobian<1, 6> Dpose2 = boost::none) const {
double result = pose_.range(pose, Dpose, Dpose2); Matrix16 Dpose;
if (Dpose) { double result = pose_.range(pose, Dcamera ? &Dpose : 0, Dpose2);
// Add columns of zeros to Jacobian for calibration if (Dcamera)
Matrix& H1r(*Dpose); *Dcamera << Dpose, Eigen::Matrix<double, 1, DimK>::Zero();
H1r.conservativeResize(Eigen::NoChange, Dim());
H1r.block(0, 6, 1, DimK) = Matrix::Zero(1, DimK);
}
return result; return result;
} }
/** /**
* Calculate range to another camera * Calculate range to another camera
* @param camera Other camera * @param camera Other camera
* @param Dpose the optionally computed Jacobian with respect to pose * @param Dcamera the optionally computed Jacobian with respect to pose
* @param Dother the optionally computed Jacobian with respect to the other camera * @param Dother the optionally computed Jacobian with respect to the other camera
* @return range (double) * @return range (double)
*/ */
template<class CalibrationB> template<class CalibrationB>
double range( double range(
const PinholeCamera<CalibrationB>& camera, // const PinholeCamera<CalibrationB>& camera, //
boost::optional<Matrix&> Dpose = boost::none, OptionalJacobian<1, DimC> Dcamera = boost::none,
boost::optional<Matrix&> Dother = boost::none) const { OptionalJacobian<1, 6 + CalibrationB::Dim()> Dother = boost::none) const {
double result = pose_.range(camera.pose_, Dpose, Dother); Matrix16 Dpose, Dpose2;
if (Dpose) { double result = pose_.range(camera.pose(), Dcamera ? &Dpose : 0,
// Add columns of zeros to Jacobian for calibration Dother ? &Dpose2 : 0);
Matrix& H1r(*Dpose); if (Dcamera)
H1r.conservativeResize(Eigen::NoChange, Dim()); *Dcamera << Dpose, Eigen::Matrix<double, 1, DimK>::Zero();
H1r.block(0, 6, 1, DimK) = Matrix::Zero(1, DimK); if (Dother)
} *Dother << Dpose2, Eigen::Matrix<double, 1, CalibrationB::DimC()>::Zero();
if (Dother) {
// Add columns of zeros to Jacobian for calibration
Matrix& H2r(*Dother);
H2r.conservativeResize(Eigen::NoChange, Dim());
H2r.block(0, 6, 1, DimK) = Matrix::Zero(1, DimK);
}
return result; return result;
} }
/** /**
* Calculate range to another camera * Calculate range to another camera
* @param camera Other camera * @param camera Other camera
* @param Dpose the optionally computed Jacobian with respect to pose * @param Dcamera the optionally computed Jacobian with respect to pose
* @param Dother the optionally computed Jacobian with respect to the other camera * @param Dother the optionally computed Jacobian with respect to the other camera
* @return range (double) * @return range (double)
*/ */
double range( double range(
const CalibratedCamera& camera, // const CalibratedCamera& camera, //
OptionalJacobian<1, 6> Dpose = boost::none, OptionalJacobian<1, DimC> Dcamera = boost::none,
OptionalJacobian<1, 6> Dother = boost::none) const { OptionalJacobian<1, 6> Dother = boost::none) const {
return pose_.range(camera.pose_, Dpose, Dother); return range(camera.pose_, Dcamera, Dother);
} }
private: private: