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
Matrix36 Dpose_;
Matrix3 Dpoint_;
Point3 q = pose_.transform_to(point, Dpose_, Dpoint_);
Point3 q = pose_.transform_to(point, Dpose ? Dpose_ : 0, Dpoint ? Dpoint_ : 0);
#else
Point3 q = pose_.transform_to(point);
#endif
@ -77,10 +77,12 @@ Point2 CalibratedCamera::project(const Point3& point,
if (Dpose || Dpoint) {
#ifdef CALIBRATEDCAMERA_CHAIN_RULE
// just implement chain rule
Matrix23 H;
project_to_camera(q,H);
if (Dpose) *Dpose = H * (*Dpose_);
if (Dpoint) *Dpoint = H * (*Dpoint_);
if(Dpose && Dpoint) {
Matrix23 H;
project_to_camera(q,H);
if (Dpose) *Dpose = H * (*Dpose_);
if (Dpoint) *Dpoint = H * (*Dpoint_);
}
#else
// optimized version, see CalibratedCamera.nb
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_,
boost::optional<Matrix&> H) {
OptionalJacobian<5,6> H) {
const Rot3& _1R2_ = _1P2_.rotation();
const Point3& _1T2_ = _1P2_.translation();
if (!H) {
@ -25,13 +25,12 @@ EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _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
// First get 2*3 derivative from Unit3::FromPoint3
Matrix D_direction_1T2;
Matrix23 D_direction_1T2;
Unit3 direction = Unit3::FromPoint3(_1T2_, D_direction_1T2);
H->resize(5, 6);
H->block<3, 3>(0, 0) << Matrix::Identity(3, 3); // upper left
H->block<2, 3>(3, 0) << Matrix::Zero(2, 3); // lower left
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<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); // upper left
H->block<2, 3>(3, 0).setZero(); // lower left
H->block<3, 3>(0, 3).setZero(); // upper right
H->block<2, 3>(3, 3) = D_direction_1T2 * _1R2_.matrix(); // lower right
return EssentialMatrix(_1R2_, direction);
}
}
@ -61,16 +60,18 @@ Vector EssentialMatrix::localCoordinates(const EssentialMatrix& other) const {
/* ************************************************************************* */
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());
Point3 q = pose.transform_to(p, DE, Dpoint);
Matrix36 DE_;
Point3 q = pose.transform_to(p, DE ? &DE_ : 0, Dpoint);
if (DE) {
// 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 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
Matrix H(3, 5);
H << DE->block<3, 3>(0, 0), -aRb_.transpose() * aTb_.basis();
Matrix35 H;
H.block<3,3>(0,0) = DE_.block<3, 3>(0, 0);
H.block<3,2>(0,3) = -aRb_.transpose() * aTb_.basis();
*DE = H;
}
return q;
@ -78,7 +79,7 @@ Point3 EssentialMatrix::transform_to(const Point3& p,
/* ************************************************************************* */
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
Rot3 c1Rc2 = aRb_.conjugate(cRb);
@ -89,18 +90,18 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb,
return EssentialMatrix(c1Rc2, c1Tc2);
} else {
// 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);
if (HE) {
*HE = zeros(5, 5);
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->setZero();
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)
}
if (HR) {
throw runtime_error(
"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<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, //
boost::optional<Matrix&> H) const {
double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, //
OptionalJacobian<1,5> H) const {
if (H) {
H->resize(1, 5);
// See math.lyx
Matrix HR = vA.transpose() * E_ * skewSymmetric(-vB);
Matrix HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB)
Matrix13 HR = vA.transpose() * E_ * skewSymmetric(-vB);
Matrix12 HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB)
* aTb_.basis();
*H << HR, HD;
H->block<1,3>(0,0) = HR;
H->block<1,2>(0,3) = HD;
}
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)
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
template<typename Engine>
@ -132,16 +132,16 @@ public:
* @return point in pose coordinates
*/
Point3 transform_to(const Point3& p,
boost::optional<Matrix&> DE = boost::none,
boost::optional<Matrix&> Dpoint = boost::none) const;
OptionalJacobian<3,5> DE = boost::none,
OptionalJacobian<3,3> Dpoint = boost::none) const;
/**
* Given essential matrix E in camera frame B, convert to body frame C
* @param cRb rotation from body frame to camera frame
* @param E essential matrix E in camera frame C
*/
EssentialMatrix rotate(const Rot3& cRb, boost::optional<Matrix&> HE =
boost::none, boost::optional<Matrix&> HR = boost::none) const;
EssentialMatrix rotate(const Rot3& cRb, OptionalJacobian<5, 5> HE =
boost::none, OptionalJacobian<5, 3> HR = boost::none) const;
/**
* Given essential matrix E in camera frame B, convert to body frame C
@ -153,8 +153,8 @@ public:
}
/// epipolar error, algebraic
double error(const Vector& vA, const Vector& vB, //
boost::optional<Matrix&> H = boost::none) const;
double error(const Vector3& vA, const Vector3& vB, //
OptionalJacobian<1,5> H = boost::none) const;
/// @}

3
gtsam/geometry/Unit3.cpp
View File

@ -39,14 +39,13 @@ using namespace std;
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);
if (H) {
// 3*3 Derivative of representation with respect to point is 3*3:
Matrix D_p_point;
point.normalize(D_p_point); // TODO, this calculates norm a second time :-(
// Calculate the 2*3 Jacobian
H->resize(2, 3);
*H << direction.basis().transpose() * D_p_point;
}
return direction;

2
gtsam/geometry/Unit3.h
View File

@ -57,7 +57,7 @@ public:
}
/// 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);
/// Random direction, using boost::uniform_on_sphere