done with geometry
Natesh Srinivasan
parent
aad0b2876b
commit
0f95890215
|
|
@ -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<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
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -30,7 +30,7 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
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
|
||||
const Point3 q = leftCamPose_.transform_to(point, H1, H2);
|
||||
|
|
@ -57,26 +57,23 @@ namespace gtsam {
|
|||
if (H1 || H2) {
|
||||
#ifdef STEREOCAMERA_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 (H2) *H2 = D_project_point*(*H2);
|
||||
#else
|
||||
// optimized version, see StereoCamera.nb
|
||||
if (H1) {
|
||||
const double v1 = v/fy, v2 = fx*v1, dx=d*x;
|
||||
*H1 = (Matrix(3, 6) <<
|
||||
uL*v1, -fx-dx*uL, v2, -dfx, 0.0, d*uL,
|
||||
*H1 << uL*v1, -fx-dx*uL, v2, -dfx, 0.0, d*uL,
|
||||
uR*v1, -fx-dx*uR, v2, -dfx, 0.0, d*uR,
|
||||
fy + v*v1, -dx*v , -x*dfy, 0.0, -dfy, d*v
|
||||
).finished();
|
||||
fy + v*v1, -dx*v , -x*dfy, 0.0, -dfy, d*v;
|
||||
}
|
||||
if (H2) {
|
||||
const Matrix R(leftCamPose_.rotation().matrix());
|
||||
*H2 = d * (Matrix(3, 3) <<
|
||||
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,
|
||||
fy*R(0, 1) - R(0, 2)*v , fy*R(1, 1) - R(1, 2)*v , fy*R(2, 1) - R(2, 2)*v
|
||||
).finished();
|
||||
const Matrix3 R(leftCamPose_.rotation().matrix());
|
||||
*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)*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;
|
||||
*H2 << d * (*H2);
|
||||
}
|
||||
#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;
|
||||
const Cal3_S2Stereo& K = *K_;
|
||||
double f_x = K.fx(), f_y = K.fy(), b = K.baseline();
|
||||
|
|
|
|||
|
|
@ -95,8 +95,8 @@ public:
|
|||
}
|
||||
|
||||
Pose3 between(const StereoCamera &camera,
|
||||
boost::optional<Matrix&> H1=boost::none,
|
||||
boost::optional<Matrix&> H2=boost::none) const {
|
||||
OptionalJacobian<6,6> H1=boost::none,
|
||||
OptionalJacobian<6,6> H2=boost::none) const {
|
||||
return leftCamPose_.between(camera.pose(), H1, H2);
|
||||
}
|
||||
|
||||
|
|
@ -119,8 +119,8 @@ public:
|
|||
* @param H3 IGNORED (for calibration)
|
||||
*/
|
||||
StereoPoint2 project(const Point3& point,
|
||||
boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none) const;
|
||||
OptionalJacobian<3, 6> H1 = boost::none,
|
||||
OptionalJacobian<3, 3> H2 = boost::none) const;
|
||||
|
||||
/**
|
||||
*
|
||||
|
|
@ -138,7 +138,7 @@ public:
|
|||
|
||||
private:
|
||||
/** 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;
|
||||
template<class Archive>
|
||||
|
|
|
|||
|
|
@ -112,14 +112,14 @@ public:
|
|||
}
|
||||
|
||||
/// 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 {
|
||||
try {
|
||||
Point2 error(camera_.project(point, boost::none, H2, boost::none) - measured_);
|
||||
return error.vector();
|
||||
} catch (CheiralityException& e) {
|
||||
if (H2)
|
||||
*H2 = zeros(2, 3);
|
||||
*H2 << Z_2x3;
|
||||
if (verboseCheirality_)
|
||||
std::cout << e.what() << ": Landmark "
|
||||
<< DefaultKeyFormatter(this->key()) << " moved behind camera"
|
||||
|
|
|
|||
|
|
@ -147,7 +147,6 @@ private:
|
|||
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
|
|
|
|||
|
|
@ -53,7 +53,7 @@ public:
|
|||
* @return Triangulated Point3
|
||||
*/
|
||||
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);
|
||||
|
||||
///
|
||||
|
|
|
|||
Loading…
Reference in New Issue