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

completed essentialmatrix.

release/4.3a0
nsrinivasan7 2014-12-02 13:49:03 -05:00
parent 22bbde6fe0
commit 7138263d85
5 changed files with 40 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/geometry/CalibratedCamera.cpp
View File

@ -64,7 +64,7 @@ Point2 CalibratedCamera::project(const Point3& point,
#ifdef CALIBRATEDCAMERA_CHAIN_RULE #ifdef CALIBRATEDCAMERA_CHAIN_RULE
Matrix36 Dpose_; Matrix36 Dpose_;
Matrix3 Dpoint_; Matrix3 Dpoint_;
Point3 q = pose_.transform_to(point, Dpose_, Dpoint_); Point3 q = pose_.transform_to(point, Dpose ? Dpose_ : 0, Dpoint ? Dpoint_ : 0);
#else #else
Point3 q = pose_.transform_to(point); Point3 q = pose_.transform_to(point);
#endif #endif
@ -77,10 +77,12 @@ Point2 CalibratedCamera::project(const Point3& point,
if (Dpose || Dpoint) { if (Dpose || Dpoint) {
#ifdef CALIBRATEDCAMERA_CHAIN_RULE #ifdef CALIBRATEDCAMERA_CHAIN_RULE
// just implement chain rule // just implement chain rule
Matrix23 H; if(Dpose && Dpoint) {
project_to_camera(q,H); Matrix23 H;
if (Dpose) *Dpose = H * (*Dpose_); project_to_camera(q,H);
if (Dpoint) *Dpoint = H * (*Dpoint_); if (Dpose) *Dpose = H * (*Dpose_);
if (Dpoint) *Dpoint = H * (*Dpoint_);
}
#else #else
// optimized version, see CalibratedCamera.nb // optimized version, see CalibratedCamera.nb
const double z = q.z(), d = 1.0 / z; const double z = q.z(), d = 1.0 / z;

47
gtsam/geometry/EssentialMatrix.cpp
View File

@ -14,7 +14,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_, EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_,
boost::optional<Matrix&> H) { OptionalJacobian<5,6> H) {
const Rot3& _1R2_ = _1P2_.rotation(); const Rot3& _1R2_ = _1P2_.rotation();
const Point3& _1T2_ = _1P2_.translation(); const Point3& _1T2_ = _1P2_.translation();
if (!H) { if (!H) {
@ -25,13 +25,12 @@ EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_,
// Calculate the 5*6 Jacobian H = D_E_1P2 // Calculate the 5*6 Jacobian H = D_E_1P2
// D_E_1P2 = [D_E_1R2 D_E_1T2], 5*3 wrpt rotation, 5*3 wrpt translation // D_E_1P2 = [D_E_1R2 D_E_1T2], 5*3 wrpt rotation, 5*3 wrpt translation
// First get 2*3 derivative from Unit3::FromPoint3 // First get 2*3 derivative from Unit3::FromPoint3
Matrix D_direction_1T2; Matrix23 D_direction_1T2;
Unit3 direction = Unit3::FromPoint3(_1T2_, D_direction_1T2); Unit3 direction = Unit3::FromPoint3(_1T2_, D_direction_1T2);
H->resize(5, 6); H->block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); // upper left
H->block<3, 3>(0, 0) << Matrix::Identity(3, 3); // upper left H->block<2, 3>(3, 0).setZero(); // lower left
H->block<2, 3>(3, 0) << Matrix::Zero(2, 3); // lower left H->block<3, 3>(0, 3).setZero(); // upper right
H->block<3, 3>(0, 3) << Matrix::Zero(3, 3); // upper right H->block<2, 3>(3, 3) = D_direction_1T2 * _1R2_.matrix(); // lower right
H->block<2, 3>(3, 3) << D_direction_1T2 * _1R2_.matrix(); // lower right
return EssentialMatrix(_1R2_, direction); return EssentialMatrix(_1R2_, direction);
} }
} }
@ -61,16 +60,18 @@ Vector EssentialMatrix::localCoordinates(const EssentialMatrix& other) const {
/* ************************************************************************* */ /* ************************************************************************* */
Point3 EssentialMatrix::transform_to(const Point3& p, Point3 EssentialMatrix::transform_to(const Point3& p,
boost::optional<Matrix&> DE, boost::optional<Matrix&> Dpoint) const { OptionalJacobian<3,5> DE, OptionalJacobian<3,3> Dpoint) const {
Pose3 pose(aRb_, aTb_.point3()); Pose3 pose(aRb_, aTb_.point3());
Point3 q = pose.transform_to(p, DE, Dpoint); Matrix36 DE_;
Point3 q = pose.transform_to(p, DE ? &DE_ : 0, Dpoint);
if (DE) { if (DE) {
// DE returned by pose.transform_to is 3*6, but we need it to be 3*5 // DE returned by pose.transform_to is 3*6, but we need it to be 3*5
// The last 3 columns are derivative with respect to change in translation // The last 3 columns are derivative with respect to change in translation
// The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis() // The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis()
// Duy made an educated guess that this needs to be rotated to the local frame // Duy made an educated guess that this needs to be rotated to the local frame
Matrix H(3, 5); Matrix35 H;
H << DE->block<3, 3>(0, 0), -aRb_.transpose() * aTb_.basis(); H.block<3,3>(0,0) = DE_.block<3, 3>(0, 0);
H.block<3,2>(0,3) = -aRb_.transpose() * aTb_.basis();
*DE = H; *DE = H;
} }
return q; return q;
@ -78,7 +79,7 @@ Point3 EssentialMatrix::transform_to(const Point3& p,
/* ************************************************************************* */ /* ************************************************************************* */
EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb,
boost::optional<Matrix&> HE, boost::optional<Matrix&> HR) const { OptionalJacobian<5,5> HE, OptionalJacobian<5,3> HR) const {
// The rotation must be conjugated to act in the camera frame // The rotation must be conjugated to act in the camera frame
Rot3 c1Rc2 = aRb_.conjugate(cRb); Rot3 c1Rc2 = aRb_.conjugate(cRb);
@ -89,18 +90,18 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb,
return EssentialMatrix(c1Rc2, c1Tc2); return EssentialMatrix(c1Rc2, c1Tc2);
} else { } else {
// Calculate derivatives // Calculate derivatives
Matrix D_c1Tc2_cRb, D_c1Tc2_aTb; // 2*3 and 2*2 Matrix23 D_c1Tc2_cRb; // 2*3
Matrix2 D_c1Tc2_aTb; // 2*2
Unit3 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb); Unit3 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb);
if (HE) { if (HE) {
*HE = zeros(5, 5); HE->setZero();
HE->block<3, 3>(0, 0) << cRb.matrix(); // a change in aRb_ will yield a rotated change in c1Rc2 HE->block<3, 3>(0, 0) = cRb.matrix(); // a change in aRb_ will yield a rotated change in c1Rc2
HE->block<2, 2>(3, 3) << D_c1Tc2_aTb; // (2*2) HE->block<2, 2>(3, 3) = D_c1Tc2_aTb; // (2*2)
} }
if (HR) { if (HR) {
throw runtime_error( throw runtime_error(
"EssentialMatrix::rotate: derivative HR not implemented yet"); "EssentialMatrix::rotate: derivative HR not implemented yet");
/* /*
HR->resize(5, 3);
HR->block<3, 3>(0, 0) << zeros(3, 3); // a change in the rotation yields ? HR->block<3, 3>(0, 0) << zeros(3, 3); // a change in the rotation yields ?
HR->block<2, 3>(3, 0) << zeros(2, 3); // (2*3) * (3*3) ? HR->block<2, 3>(3, 0) << zeros(2, 3); // (2*3) * (3*3) ?
*/ */
@ -110,15 +111,15 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb,
} }
/* ************************************************************************* */ /* ************************************************************************* */
double EssentialMatrix::error(const Vector& vA, const Vector& vB, // double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, //
boost::optional<Matrix&> H) const { OptionalJacobian<1,5> H) const {
if (H) { if (H) {
H->resize(1, 5);
// See math.lyx // See math.lyx
Matrix HR = vA.transpose() * E_ * skewSymmetric(-vB); Matrix13 HR = vA.transpose() * E_ * skewSymmetric(-vB);
Matrix HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB) Matrix12 HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB)
* aTb_.basis(); * aTb_.basis();
*H << HR, HD; H->block<1,3>(0,0) = HR;
H->block<1,2>(0,3) = HD;
} }
return dot(vA, E_ * vB); return dot(vA, E_ * vB);
} }

14
gtsam/geometry/EssentialMatrix.h
View File

@ -50,7 +50,7 @@ public:
/// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale) /// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale)
static EssentialMatrix FromPose3(const Pose3& _1P2_, static EssentialMatrix FromPose3(const Pose3& _1P2_,
boost::optional<Matrix&> H = boost::none); OptionalJacobian<5, 6> H = boost::none);
/// Random, using Rot3::Random and Unit3::Random /// Random, using Rot3::Random and Unit3::Random
template<typename Engine> template<typename Engine>
@ -132,16 +132,16 @@ public:
* @return point in pose coordinates * @return point in pose coordinates
*/ */
Point3 transform_to(const Point3& p, Point3 transform_to(const Point3& p,
boost::optional<Matrix&> DE = boost::none, OptionalJacobian<3,5> DE = boost::none,
boost::optional<Matrix&> Dpoint = boost::none) const; OptionalJacobian<3,3> Dpoint = boost::none) const;
/** /**
* Given essential matrix E in camera frame B, convert to body frame C * Given essential matrix E in camera frame B, convert to body frame C
* @param cRb rotation from body frame to camera frame * @param cRb rotation from body frame to camera frame
* @param E essential matrix E in camera frame C * @param E essential matrix E in camera frame C
*/ */
EssentialMatrix rotate(const Rot3& cRb, boost::optional<Matrix&> HE = EssentialMatrix rotate(const Rot3& cRb, OptionalJacobian<5, 5> HE =
boost::none, boost::optional<Matrix&> HR = boost::none) const; boost::none, OptionalJacobian<5, 3> HR = boost::none) const;
/** /**
* Given essential matrix E in camera frame B, convert to body frame C * Given essential matrix E in camera frame B, convert to body frame C
@ -153,8 +153,8 @@ public:
} }
/// epipolar error, algebraic /// epipolar error, algebraic
double error(const Vector& vA, const Vector& vB, // double error(const Vector3& vA, const Vector3& vB, //
boost::optional<Matrix&> H = boost::none) const; OptionalJacobian<1,5> H = boost::none) const;
/// @} /// @}

3
gtsam/geometry/Unit3.cpp
View File

@ -39,14 +39,13 @@ using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Unit3 Unit3::FromPoint3(const Point3& point, boost::optional<Matrix&> H) { Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) {
Unit3 direction(point); Unit3 direction(point);
if (H) { if (H) {
// 3*3 Derivative of representation with respect to point is 3*3: // 3*3 Derivative of representation with respect to point is 3*3:
Matrix D_p_point; Matrix D_p_point;
point.normalize(D_p_point); // TODO, this calculates norm a second time :-( point.normalize(D_p_point); // TODO, this calculates norm a second time :-(
// Calculate the 2*3 Jacobian // Calculate the 2*3 Jacobian
H->resize(2, 3);
*H << direction.basis().transpose() * D_p_point; *H << direction.basis().transpose() * D_p_point;
} }
return direction; return direction;

2
gtsam/geometry/Unit3.h
View File

@ -57,7 +57,7 @@ public:
} }
/// Named constructor from Point3 with optional Jacobian /// Named constructor from Point3 with optional Jacobian
static Unit3 FromPoint3(const Point3& point, boost::optional<Matrix&> H = static Unit3 FromPoint3(const Point3& point, OptionalJacobian<2,3> H =
boost::none); boost::none);
/// Random direction, using boost::uniform_on_sphere /// Random direction, using boost::uniform_on_sphere