diff --git a/gtsam/slam/MultiDisparityFactor.cpp b/gtsam/slam/MultiDisparityFactor.cpp deleted file mode 100644 index 8b53a680c..000000000 --- a/gtsam/slam/MultiDisparityFactor.cpp +++ /dev/null @@ -1,111 +0,0 @@ -/* - * MultiDisparityFactor.cpp - * - * Created on: Jan 30, 2014 - * Author: nsrinivasan7 - */ - - -#include "MultiDisparityFactor.h" -#include - - -using namespace std; - -namespace gtsam { - -//*************************************************************************** - -void MultiDisparityFactor::print(const string& s) const { - cout << "Prior Factor on " << landmarkKey_ << "\n"; - - 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"; -}; - -//*************************************************************************** - -Vector MultiDisparityFactor::evaluateError(const OrientedPlane3& plane, - boost::optional H) const { - Vector e; - e.resize(uv_.rows()); - if(H) { - (*H).resize(uv_.rows(), 3); - - Matrix B; - B.resize(4,3); - B.block<3,2>(0,0) << plane.normal().basis(); - B.block<4,1>(0,2) << 0 , 0 , 0 ,1; - B.block<1,2>(3,0) << 0 , 0 ; - 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) ) ) * B; - - } - 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(),4); - Matrix wRc = cameraPose_.rotation().matrix(); - Rn_.setZero(); - Rn_ << -1.0 *uv_ * wRc.transpose(); - - return; -} - -//*************************************************************************** - -void MultiDisparityFactor::Rd(const OrientedPlane3& p) const { - - Rd_.resize(1,4); - Vector wTc = cameraPose_.translation().vector(); - - - Rd_.block<1,3>(0,0) << wTc.transpose(); - Rd_.block<1,1>(0,3) << 1.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 = Rn_.row(i) * planecoeffs; - Matrix denominator = Rd_ * planecoeffs; - - // cout << "Numerator : " << numerator << " \t Denominator :" << denominator << "\n"; - e(i,0) = disparities_(i,0) - ( ( 1.0 * numerator(0,0) ) / ( denominator(0,0) ) ); -// cout << e(i,0) << " = " << disparities_(i,0) << " - " << ( numerator(0,0) /( denominator(0,0) ) ) << "\n"; - } - return e; - -} - -//*************************************************************************** -} diff --git a/gtsam/slam/MultiDisparityFactor.h b/gtsam/slam/MultiDisparityFactor.h deleted file mode 100644 index c647e87d5..000000000 --- a/gtsam/slam/MultiDisparityFactor.h +++ /dev/null @@ -1,103 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/* - * @file MultiDisparityFactor.h - * @date Jan 30, 2013 - * @author Natesh Srinivasan - * @brief A factor for modeling the disparity across multiple views - */ - -#pragma once - -#include -#include - - -namespace gtsam { - -/** - * Unary factor on measured disparity from multiple views as deterministic function of camera pose - */ - -class MultiDisparityFactor: public NoiseModelFactor1 { - - protected : - - Key landmarkKey_; // the key of the hidden plane in the world - 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 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 Rn_; // the numerator matrix - mutable std::vector > R_; - - typedef NoiseModelFactor1 Base; - - public: - - // Constructor - MultiDisparityFactor() - {}; - - /// Constructor with measured plane coefficients (a,b,c,d), noise model, pose symbol - MultiDisparityFactor (Key key, const Vector& disparities, const Eigen::Matrix& uv, - const gtsam::Pose3& cameraPose,const SharedIsotropic& noiseModel) - : Base (noiseModel, key), - landmarkKey_ (key), - disparities_(disparities), - uv_(uv), - cameraPose_(cameraPose) - {}; - - - /// print - void print(const std::string& s="Multi-View DisaprityFactor") const; - - virtual Vector evaluateError(const OrientedPlane3& plane, - boost::optional 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 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 diff --git a/gtsam/slam/tests/testMultiDisparityFactor.cpp b/gtsam/slam/tests/testMultiDisparityFactor.cpp deleted file mode 100644 index 34e34f9b0..000000000 --- a/gtsam/slam/tests/testMultiDisparityFactor.cpp +++ /dev/null @@ -1,274 +0,0 @@ - - /* testMultiDisparityFactor.cpp - * - * Created on: Jan 31, 2014 - * Author: nsrinivasan7 - * @brief: Unittest for MultidisparityFactor - */ - - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace boost::assign; -using namespace gtsam; -using namespace std; - -GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3) -GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3) - -//void generateDisparities(Eigen::Matrix& uv, Vector& disparity, Pose3& cameraPose) { - -// double w = 640.0; -// double h = 480.0; -// double beta = 0.1; - -// double alphax = 700.0; -// double alphay = 700.0; -// double f = (alphax + alphay)/2.0; - -// Matrix Rot = cameraPose.rotation().matrix(); -// Vector trans = cameraPose.translation().vector(); - -// // plane parameters -// Matrix norm; -// norm.resize(1,3); -// norm << 1/sqrt(2), 0.0, -1/sqrt(2); -// double d = 20.0; - -// uv.resize(w*h,3); - -// disparity.resize(w*h); -// for(int u = 0; u < w; u++) -// for(int v = 0; v < h ; v++) { -// uv.row(v*w+u) << Matrix_(1,3, (double)u, (double)v, f*beta); -// Matrix l = norm * trans; -// Matrix disp = ( -1.0/(l(0,0) + d) ) * norm * Rot * ( uv.row(v*w+u).transpose() ); - -// disparity(v*w+u,0) = disp(0,0); -// } - -//} - -//TEST(MutliDisparityFactor,Rd) -//{ - -// Key key(1); -// Vector disparities = Vector_(2, 1.0, 1.0); // matlab generated values - -// Eigen::Matrix 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.Rd(p); -// Matrix actualRd = factor.Rd(); -// Matrix expectedRd = Matrix_(1,4,1.0,1.0,1.0,1.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 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); - -// 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,4, -20.0, -30.0, -70.0, 0.0, -40.0, -60.0, -70.0, 0.0); - -// EXPECT(assert_equal( expectedRn,actualRn,1e-8) ); -//} - -//// unit test for derivative -//TEST(MutliDisparityFactor,H) -//{ -// Key key(1); -// Vector disparities = Vector_(2, -3.6123, -4.4910); // matlab generated values - -// Eigen::Matrix 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( -// boost::bind(&MultiDisparityFactor::evaluateError, &factor, _1, boost::none), p1); - -// EXPECT(assert_equal( expectedH,actualH,1e-8) ); -//} - -//// unit test for optimization -//TEST(MultiDisparityFactor,optimize) { - -// NonlinearFactorGraph graph; - -// Vector disparities; -// Eigen::Matrix uv; - -// gtsam::Rot3 R = gtsam::Rot3(); -// gtsam::Pose3 cameraPose( R.RzRyRx(0,-M_PI/3,0) , gtsam::Point3(50.0, 0.0, 50.0) ); - -// generateDisparities(uv,disparities,cameraPose); - -// Key key(1); -// SharedIsotropic model = gtsam::noiseModel::Isotropic::Sigma(disparities.rows(), 0.25, true); -// MultiDisparityFactor factor1(key, disparities, uv, cameraPose, model); -// graph.push_back(factor1); - -// Values initialEstimate; -// initialEstimate.insert(1, OrientedPlane3( 1.0/sqrt(2) + 0.2, 0.3, -1.0/sqrt(2) - 0.2, 20.0 ) ); - -// GaussNewtonParams parameters; -// // Stop iterating once the change in error between steps is less than this value -// parameters.relativeErrorTol = 1e-5; -// // Do not perform more than N iteration steps -// parameters.maxIterations = 1000; -// // Create the optimizer ... -// GaussNewtonOptimizer optimizer(graph, initialEstimate, parameters); -// // ... and optimize -// Values actualresult = optimizer.optimize(); - -// Values expectedresult; -// expectedresult.insert(1, OrientedPlane3( 1.0/sqrt(2), 0.0, -1.0/sqrt(2), 20.0 ) ); - -// EXPECT(assert_equal( expectedresult,actualresult,1e-8) ); -//} - -//// model selection test with two models -//TEST(MultiDisparityFactor,modelselect) -//{ - -// // ************************Image 1 -// Vector disparities1; -// Eigen::Matrix uv1; - -// gtsam::Rot3 R1 = gtsam::Rot3(); -// gtsam::Pose3 cameraPose1( R1.RzRyRx(0,-M_PI/3,0) , gtsam::Point3(50.0, 0.0, 50.0) ); - -// generateDisparities(uv1,disparities1,cameraPose1); - -// // ***************************Image 2 -// NonlinearFactorGraph graph2; - -// Vector disparities2; -// Eigen::Matrix uv2; - -// gtsam::Rot3 R2 = gtsam::Rot3(); -// gtsam::Pose3 cameraPose2( R2.RzRyRx(0,-M_PI/4,0) , gtsam::Point3(30.0, 0.0, 20.0) ); - -// generateDisparities(uv2,disparities2,cameraPose2); - -// // ****************************Model 1 - -// NonlinearFactorGraph graph1; -// Key key1(1); -// SharedIsotropic model1 = gtsam::noiseModel::Isotropic::Sigma(disparities1.rows(), 0.25, true); -// MultiDisparityFactor factor1(key1, disparities1, uv1, cameraPose1, model1); -// graph1.push_back(factor1); - -// Values initialEstimate1; -// initialEstimate1.insert(1, OrientedPlane3( 1.0/sqrt(2) + 0.2, 0.3, -1.0/sqrt(2) - 0.2, 20.0 ) ); - -// GaussNewtonParams parameters1; -// // Stop iterating once the change in error between steps is less than this value -// parameters1.relativeErrorTol = 1e-5; -// // Do not perform more than N iteration steps -// parameters1.maxIterations = 1000; -// // Create the optimizer ... -// GaussNewtonOptimizer optimizer1(graph1, initialEstimate1, parameters1); -// // ... and optimize -// Values result1 = optimizer1.optimize(); - -// Marginals marginals1(graph1, result1); -// print(marginals1.marginalCovariance(1), "Theta1 Covariance"); - -// // ****************************Model 2 - -//// Key key2(1); -//// SharedIsotropic model2 = gtsam::noiseModel::Isotropic::Sigma(disparities2.rows(), 0.25, true); -//// MultiDisparityFactor factor2(key2, disparities2, uv2, cameraPose2, model2); -//// graph2.push_back(factor2); -//// -//// Values initialEstimate2; -//// initialEstimate2.insert(1, OrientedPlane3( 1.0/sqrt(2) + 0.2, 0.3, -1.0/sqrt(2) - 0.2, 20.0 ) ); -//// -//// GaussNewtonParams parameters2; -//// // Stop iterating once the change in error between steps is less than this value -//// parameters2.relativeErrorTol = 1e-5; -//// // Do not perform more than N iteration steps -//// parameters2.maxIterations = 1000; -//// // Create the optimizer ... -//// GaussNewtonOptimizer optimizer2(graph2, initialEstimate2, parameters2); -//// // ... and optimize -//// Values actualresult2 = optimizer2.optimize(); -//// -//// Values expectedresult2; -//// expectedresult2.insert(1, OrientedPlane3( 1.0/sqrt(2), 0.0, -1.0/sqrt(2), 20.0 ) ); -//// -//// Values result2 = optimizer2.optimize(); -//// -//// Marginals marginals2(graph2, result2); -//// print(marginals2.marginalCovariance(2), "Theta2 Covariance"); - -//} -/* ************************************************************************* */ -int main() { - srand(time(NULL)); - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */