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

Added camera pose and image co-ordinate measurements

release/4.3a0
Natesh Srinivasan 2014-01-31 18:11:50 -05:00
parent a75df298d7
commit 2817a05911
3 changed files with 25 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/slam/MultiDisparityFactor.cpp
View File

@ -18,8 +18,14 @@ namespace gtsam {
void MultiDisparityFactor::print(const string& s) const {
cout << "Prior Factor on " << landmarkKey_ << "\n";
cout << "Measured Disparities : \n " << disparities_ << "\n";
this->noiseModel_->print(" Noise model: ");
for(int i = 0; i < disparities_.rows(); i++) {
cout << "Disparity @ (" << uv_(i,0) << ", " << uv_(i,1) << ") = " << disparities_(i) << "\n";
}
cameraPose_.print("Camera Pose ");
this->noiseModel_->print(" noise model: ");
cout << "\n";
};
//***************************************************************************
@ -27,7 +33,7 @@ void MultiDisparityFactor::print(const string& s) const {
Vector MultiDisparityFactor::evaluateError(const OrientedPlane3& plane,
boost::optional<Matrix&> H) const {
return Vector_(3,1,1,1);
};
}

13
gtsam/slam/MultiDisparityFactor.h
View File

@ -33,7 +33,10 @@ class MultiDisparityFactor: public NoiseModelFactor1<OrientedPlane3> {
protected :
Key landmarkKey_; // the key of the hidden plane in the world
Vector disparities_; // measured disparities in a set of Superpixels \mathcal{V}
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
// aligned with the disparity
typedef NoiseModelFactor1<OrientedPlane3> Base;
@ -44,11 +47,13 @@ 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 SharedIsotropic& noiseModel)
MultiDisparityFactor (Key key, const Vector& disparities, const Eigen::Matrix<int,Eigen::Dynamic,2>& uv,
const Pose3& cameraPose, const SharedIsotropic& noiseModel)
: Base (noiseModel, key),
landmarkKey_ (key),
disparities_(disparities)
disparities_(disparities),
uv_(uv),
cameraPose_(cameraPose)
{};

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

@ -41,12 +41,16 @@ TEST(MutliDisparityFactor,error)
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;
SharedIsotropic model = gtsam::noiseModel::Isotropic::Sigma(disparities.rows(), 0.25, true);
cout << "Vector # main :" << disparities << endl;
Pose3 cameraPose;
MultiDisparityFactor factor(key, disparities, uv, cameraPose, model);
factor.print("Multi Disparity factor");
MultiDisparityFactor factor(key, disparities, model);
factor.print("Multi-disparity Factor");
}
/* ************************************************************************* */