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

Added Multi Disparity Factor

release/4.3a0
nsrinivasan7 2014-02-03 10:52:10 -05:00
parent 2817a05911
commit 24fb1cc48c
3 changed files with 204 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
gtsam/slam/MultiDisparityFactor.cpp
View File

@ -32,8 +32,63 @@ void MultiDisparityFactor::print(const string& s) const {
Vector MultiDisparityFactor::evaluateError(const OrientedPlane3& plane,
boost::optional<Matrix&> H) const {
Vector e;
vector p2 = plane.normal().basis().transpose()
e.resize(uv_.rows());
if(H) {
(*H).resize(uv_.rows(), 3);
R(plane);
for(int i = 0 ; i < uv_.rows() ; i++ ) {
Matrix d = Rd_ * plane.planeCoefficients();
(*H).row(i) = (plane.planeCoefficients().transpose() * R_.at(i) ) /( pow(d(0,0) ,2) );
}
e = diff(plane);
return e;
} else {
R(plane); // recompute the Rd_, R_, Rn_
e = diff(plane);
return e;
}
}
};
void MultiDisparityFactor::Rn(const OrientedPlane3& p) const {
Rn_.resize(uv_.rows(),3);
Matrix wRc = cameraPose_.rotation().matrix();
Rn_.setZero();
Rn_ << uv_ * wRc.transpose() * p.normal().basis();
return;
}
void MultiDisparityFactor::Rd(const OrientedPlane3& p) const {
Rd_.resize(1,3);
Vector wTc = cameraPose_.translation().vector();
Rd_.block<1,2>(0,0) << wTc.transpose() * p.normal().basis();
Rd_.block<1,1>(0,2) << 0.0;
return;
}
Vector MultiDisparityFactor::diff(const OrientedPlane3& theta) const {
Vector e;
e.resize(uv_.rows(),1);
Matrix wRc = cameraPose_.rotation().matrix();
Vector wTc = cameraPose_.translation().vector();
Vector planecoeffs = theta.planeCoefficients();
for(int i=0; i < uv_.rows(); i++) {
Matrix numerator = planecoeffs.block(0,0,3,1).transpose() * wRc * uv_.row(i).transpose();
Matrix denominator = planecoeffs.block(0,0,3,1).transpose() * wTc;
cout << "Numerator : \n" << numerator << "Denominator \n" << denominator <<"\n";
e(i,0) = disparities_(i,0) - ( numerator(0,0) /( denominator(0,0) + planecoeffs(0,3) ) );
cout << e(i,0) << " = " << disparities_(i,0) << " - " << ( numerator(0,0) /( denominator(0,0) + planecoeffs(0,3) ) ) << "\n";
}
cout << "\n";
return e;
}
}

43
gtsam/slam/MultiDisparityFactor.h
View File

@ -33,11 +33,15 @@ class MultiDisparityFactor: public NoiseModelFactor1<OrientedPlane3> {
protected :
Key landmarkKey_; // the key of the hidden plane in the world
Pose3 cameraPose_; // not a random variable , treated as a parameter to the factor
gtsam::Pose3 cameraPose_; // not a random variable , treated as a parameter to the factor
Vector disparities_; // measured disparity at a Pixel (u,v)
Eigen::Matrix<int,Eigen::Dynamic,2> uv_; // the 2D image coordinates. It is assumed here that the image co-ordinates are
Eigen::Matrix<double,Eigen::Dynamic,3> uv_; // the 2D image coordinates. It is assumed here that the image co-ordinates are
// aligned with the disparity
mutable Eigen::MatrixXd Rd_; // the denominator matrix
mutable Eigen::Matrix<double, Eigen::Dynamic, 3> Rn_; // the numerator matrix
mutable std::vector<Eigen::Matrix<double,3,3> > R_;
typedef NoiseModelFactor1<OrientedPlane3> Base;
public:
@ -47,8 +51,8 @@ class MultiDisparityFactor: public NoiseModelFactor1<OrientedPlane3> {
{};
/// Constructor with measured plane coefficients (a,b,c,d), noise model, pose symbol
MultiDisparityFactor (Key key, const Vector& disparities, const Eigen::Matrix<int,Eigen::Dynamic,2>& uv,
const Pose3& cameraPose, const SharedIsotropic& noiseModel)
MultiDisparityFactor (Key key, const Vector& disparities, const Eigen::Matrix<double,Eigen::Dynamic,3>& uv,
const gtsam::Pose3& cameraPose,const SharedIsotropic& noiseModel)
: Base (noiseModel, key),
landmarkKey_ (key),
disparities_(disparities),
@ -63,6 +67,37 @@ class MultiDisparityFactor: public NoiseModelFactor1<OrientedPlane3> {
virtual Vector evaluateError(const OrientedPlane3& plane,
boost::optional<Matrix&> H1 = boost::none) const;
void Rn(const OrientedPlane3& p) const;
inline const Eigen::MatrixXd Rn() {
return Rn_;
}
void Rd(const OrientedPlane3& p) const;
inline const Eigen::MatrixXd Rd() {
return Rd_;
}
void R(const OrientedPlane3& p) const {
Rd(p);
Rn(p);
for(int i =0; i < Rn_.rows(); i++) {
Matrix Rnr = Rn_.row(i);
R_.push_back( Rd_.transpose() * Rnr - Rnr.transpose() * Rd_ );
}
}
inline const Eigen::Matrix<double,3,3> getR(int i) {
return R_.at(i);
}
bool equals(const NonlinearFactor &p, double tol = 1e-9) const {
}
// compute the differene between predivted and actual disparity
Vector diff(const OrientedPlane3& theta) const;
};
} // gtsam

115
gtsam/slam/tests/testMultiDisparityFactor.cpp
View File

@ -35,22 +35,125 @@ using namespace std;
GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3)
GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3)
TEST(MutliDisparityFactor,error)
TEST(MutliDisparityFactor,Rd)
{
Key key(1);
Vector disparities = Vector_(2, 1.0, 1.0); // matlab generated values
Eigen::Matrix<int,Eigen::Dynamic,2> uv;
uv.resize(2,2);
uv.block<2,2>(0,0) << 20, 30, 40, 60;
Eigen::Matrix<double,Eigen::Dynamic,3> uv;
uv.resize(2,3);
uv.block<2,3>(0,0) << 20.0, 30.0, 70.0, 40.0, 60.0, 70.0;
SharedIsotropic model = gtsam::noiseModel::Isotropic::Sigma(disparities.rows(), 0.25, true);
Pose3 cameraPose;
gtsam::Pose3 cameraPose( gtsam::Rot3(), gtsam::Point3(1.0, 1.0, 1.0) );
MultiDisparityFactor factor(key, disparities, uv, cameraPose, model);
factor.print("Multi Disparity factor");
// basis = [0 1 0; -1 0 0]
Vector theta = Vector_(4,0.0,0.0,1.0,20.0);
OrientedPlane3 p(theta);
factor.Rd(p);
Matrix actualRd = factor.Rd();
Matrix expectedRd = Matrix_(1,3,1.0,-1.0,0.0);
EXPECT(assert_equal( expectedRd,actualRd,1e-8) );
}
TEST(MutliDisparityFactor,Rn)
{
Key key(1);
Vector disparities = Vector_(2, 1.0, 1.0); // matlab generated values
Eigen::Matrix<double,Eigen::Dynamic,3> uv;
uv.resize(2,3);
uv.block<2,3>(0,0) << 20.0, 30.0, 70.0, 40.0, 60.0, 70.0;
SharedIsotropic model = gtsam::noiseModel::Isotropic::Sigma(disparities.rows(), 0.25, true);
gtsam::Pose3 cameraPose( gtsam::Rot3(), gtsam::Point3(1.0, 1.0, 1.0) );
MultiDisparityFactor factor(key, disparities, uv, cameraPose, model);
// basis = [0 1 0; -1 0 0]
Vector theta = Vector_(4,0.0,0.0,1.0,20.0);
OrientedPlane3 p(theta);
factor.Rn(p);
Matrix actualRn = factor.Rn();
Matrix expectedRn = Matrix_(2,3, 30.0, -20.0, 0.0, 60.0, -40.0, 0.0);
EXPECT(assert_equal( expectedRn,actualRn,1e-8) );
}
TEST(MutliDisparityFactor,R)
{
Key key(1);
Vector disparities = Vector_(2, 1.0, 1.0); // matlab generated values
Eigen::Matrix<double,Eigen::Dynamic,3> uv;
uv.resize(2,3);
uv.block<2,3>(0,0) << 20.0, 30.0, 70.0, 40.0, 60.0, 70.0;
SharedIsotropic model = gtsam::noiseModel::Isotropic::Sigma(disparities.rows(), 0.25, true);
gtsam::Pose3 cameraPose( gtsam::Rot3(), gtsam::Point3(1.0, 1.0, 1.0) );
MultiDisparityFactor factor(key, disparities, uv, cameraPose, model);
// basis = [0 1 0; -1 0 0]
Vector theta = Vector_(4,0.0,0.0,1.0,20.0);
OrientedPlane3 p(theta);
factor.Rn(p);
factor.Rd(p);
factor.R(p);
Matrix expectedR;
expectedR.resize(3,3);
expectedR << 0, 10, 0,
-10, 0, 0,
0, 0, 0;
Matrix actualR = factor.getR(0);
EXPECT(assert_equal( expectedR,actualR,1e-8) );
expectedR << 0, 20, 0,
-20, 0, 0,
0, 0, 0;
actualR = factor.getR(1);
EXPECT(assert_equal( expectedR,actualR,1e-8) );
}
TEST(MutliDisparityFactor,H)
{
Key key(1);
Vector disparities = Vector_(2, 20.0, 40.0); // matlab generated values
Eigen::Matrix<double,Eigen::Dynamic,3> uv;
uv.resize(2,3);
uv.block<2,3>(0,0) << 20.0, 30.0, 70.0, 40.0, 60.0, 70.0;
SharedIsotropic model = gtsam::noiseModel::Isotropic::Sigma(disparities.rows(), 0.25, true);
gtsam::Pose3 cameraPose( gtsam::Rot3(), gtsam::Point3(1.0, 1.0, 1.0) );
MultiDisparityFactor factor(key, disparities, uv, cameraPose, model);
// basis = [0 1 0; -1 0 0]
Vector theta = Vector_(4,0.25,1.75,1.0,20.0);
OrientedPlane3 p(theta);
Matrix actualH;
factor.R(p);
Vector theta1 = Vector_(4,0.45,0.45,1.0,20.0);
OrientedPlane3 p1(theta1);
Vector err = factor.evaluateError(p1,actualH);
Matrix expectedH = numericalDerivative11<OrientedPlane3>(
boost::bind(&MultiDisparityFactor::evaluateError, &factor, _1, boost::none), p1);
cout << "expectedH :" << expectedH << "\n";
cout << "actualH :" << actualH << "\n";
// EXPECT(assert_equal( expectedH,actualH,1e-8) );
}
/* ************************************************************************* */