From 734f0fbdf327195cd19f527b219885d4e66b0d09 Mon Sep 17 00:00:00 2001 From: Natesh Srinivasan Date: Mon, 3 Feb 2014 17:26:09 -0500 Subject: [PATCH] Working copy of multi view disparity factor --- gtsam/slam/MultiDisparityFactor.cpp | 35 +-- gtsam/slam/tests/testMultiDisparityFactor.cpp | 201 ++++++++++++++---- 2 files changed, 178 insertions(+), 58 deletions(-) diff --git a/gtsam/slam/MultiDisparityFactor.cpp b/gtsam/slam/MultiDisparityFactor.cpp index 07f66bfa7..8b53a680c 100644 --- a/gtsam/slam/MultiDisparityFactor.cpp +++ b/gtsam/slam/MultiDisparityFactor.cpp @@ -41,12 +41,13 @@ Vector MultiDisparityFactor::evaluateError(const OrientedPlane3& plane, 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,3>(3,0) << 0 , 0 , 0; + 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; + (*H).row(i) = ( (plane.planeCoefficients().transpose() * R_.at(i) ) /(pow(d(0,0) ,2) ) ) * B; + } e = diff(plane); return e; @@ -55,29 +56,38 @@ Vector MultiDisparityFactor::evaluateError(const OrientedPlane3& plane, 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_ << uv_ * wRc.transpose(); + 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) << -1 * wTc.transpose(); - Rd_.block<1,1>(0,3) << 0.0; + + 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); @@ -85,16 +95,17 @@ Vector MultiDisparityFactor::diff(const OrientedPlane3& theta) const { 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 << "\n Plane Normals : " << planecoeffs.block(0,0,3,1); - cout << "\nNumerator : " << numerator(0,0) << "\n Denominator : " << denominator(0,0) << "\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"; + + 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"; } - cout << "\n"; return e; } +//*************************************************************************** } diff --git a/gtsam/slam/tests/testMultiDisparityFactor.cpp b/gtsam/slam/tests/testMultiDisparityFactor.cpp index b26ed92ef..e99588e49 100644 --- a/gtsam/slam/tests/testMultiDisparityFactor.cpp +++ b/gtsam/slam/tests/testMultiDisparityFactor.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include #include @@ -35,6 +36,39 @@ 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) { @@ -55,8 +89,8 @@ TEST(MutliDisparityFactor,Rd) 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) ); + Matrix expectedRd = Matrix_(1,4,1.0,1.0,1.0,1.0); + EXPECT(assert_equal( expectedRd,actualRd,1e-8) ); } @@ -75,57 +109,20 @@ TEST(MutliDisparityFactor,Rn) 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); + 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) ); -} - -TEST(MutliDisparityFactor,R) -{ - 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.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) ); + EXPECT(assert_equal( expectedRn,actualRn,1e-8) ); } +// unit test for derivative TEST(MutliDisparityFactor,H) { Key key(1); - Vector disparities = Vector_(2, 20.0, 40.0); // matlab generated values + Vector disparities = Vector_(2, -3.6123, -4.4910); // matlab generated values Eigen::Matrix uv; uv.resize(2,3); @@ -151,11 +148,123 @@ TEST(MutliDisparityFactor,H) Matrix expectedH = numericalDerivative11( boost::bind(&MultiDisparityFactor::evaluateError, &factor, _1, boost::none), p1); - cout << "expectedH :" << expectedH << "\n"; - cout << "actualH :" << actualH << "\n"; -// EXPECT(assert_equal( expectedH,actualH,1e-8) ); + 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));