Working copy of multi view disparity factor
Natesh Srinivasan
parent
46b6942fd1
commit
734f0fbdf3
|
|
@ -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;
|
||||
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
}
|
||||
|
|
|
|||
|
|
@ -18,6 +18,7 @@
|
|||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/Marginals.h>
|
||||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
|
|
@ -35,6 +36,39 @@ using namespace std;
|
|||
GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3)
|
||||
GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3)
|
||||
|
||||
void generateDisparities(Eigen::Matrix<double,Eigen::Dynamic,3>& 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<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) );
|
||||
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<double,Eigen::Dynamic,3> uv;
|
||||
uv.resize(2,3);
|
||||
|
|
@ -151,11 +148,123 @@ TEST(MutliDisparityFactor,H)
|
|||
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) );
|
||||
EXPECT(assert_equal( expectedH,actualH,1e-8) );
|
||||
}
|
||||
|
||||
// unit test for optimization
|
||||
TEST(MultiDisparityFactor,optimize) {
|
||||
|
||||
NonlinearFactorGraph graph;
|
||||
|
||||
Vector disparities;
|
||||
Eigen::Matrix<double,Eigen::Dynamic,3> 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<double,Eigen::Dynamic,3> 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<double,Eigen::Dynamic,3> 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));
|
||||
|
|
|
|||
Loading…
Reference in New Issue