typename boost::math::tools::promote_args<T_y, T_shape>::type
lkj_corr_log(const Eigen::Matrix<T_y, Eigen::Dynamic, Eigen::Dynamic>& y,
const T_shape& eta) {
static const char* function("lkj_corr_log");
using boost::math::tools::promote_args;
typename promote_args<T_y, T_shape>::type lp(0.0);
check_positive(function, "Shape parameter", eta);
check_corr_matrix(function, "Correlation matrix", y);
const unsigned int K = y.rows();
if (K == 0)
return 0.0;
if (include_summand<propto, T_shape>::value)
lp += do_lkj_constant(eta, K);
if ( (eta == 1.0) &&
stan::is_constant<typename stan::scalar_type<T_shape> >::value )
return lp;
if (!include_summand<propto, T_y, T_shape>::value)
return lp;
Eigen::Matrix<T_y, Eigen::Dynamic, 1> values =
y.ldlt().vectorD().array().log().matrix();
lp += (eta - 1.0) * sum(values);
return lp;
}
void ConstraintManager::computeConstraintForces()
{
for (std::vector< FixedDistanceConstraint* >::iterator iter = m_constraintForces.begin(); iter < m_constraintForces.end(); ++iter)
{
(*iter)->populateConstraintMatrix( m_constraint );
(*iter)->populateJacobian( m_jacobian );
(*iter)->populateJacobianDerivative( m_jacobianDeriv );
}
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > invMassMatrix;
Eigen::Matrix< double, Eigen::Dynamic, 1 > velocityCurrent;
Eigen::Matrix< double, Eigen::Dynamic, 1 > forceCurrent;
m_particleManager.getInvMassMatrix( invMassMatrix );
m_particleManager.getCurrentState( velocityCurrent, false, true );
m_particleManager.getCurrentDerivative( forceCurrent, false, true );
m_constraintDeriv = m_jacobian * velocityCurrent;
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > jacobianInvJacobianT = m_jacobian * invMassMatrix * m_jacobian.transpose();
Eigen::Matrix< double, Eigen::Dynamic, 1 > jacobianDerivVelocity = m_jacobianDeriv * velocityCurrent;
Eigen::Matrix< double, Eigen::Dynamic, 1 > jacobianInvMassForce = m_jacobian * invMassMatrix * forceCurrent;
Eigen::Matrix< double, Eigen::Dynamic, 1 > b = -jacobianDerivVelocity - jacobianInvMassForce - m_ks*m_constraint - m_kd*m_constraintDeriv;
if (jacobianInvJacobianT.determinant() != 0)
{
m_lagrangeMultipliers = jacobianInvJacobianT.ldlt().solve( b );
}
else
{
m_lagrangeMultipliers.resize( m_constraintForces.size() );
m_lagrangeMultipliers.setZero();
}
}
inline bool check_pos_definite(const char* function,
const Eigen::Matrix<T_y,Eigen::Dynamic,Eigen::Dynamic>& y,
const char* name,
T_result* result) {
typedef
typename Eigen::Matrix<T_y,Eigen::Dynamic,Eigen::Dynamic>::size_type
size_type;
if (y.rows() == 1 && y(0,0) <= CONSTRAINT_TOLERANCE) {
std::ostringstream message;
message << name << " is not positive definite. "
<< name << "(0,0) is %1%.";
std::string msg(message.str());
return dom_err(function,y(0,0),name,msg.c_str(),"",result);
}
Eigen::LDLT< Eigen::Matrix<T_y,Eigen::Dynamic,Eigen::Dynamic> > cholesky
= y.ldlt();
if(cholesky.info() != Eigen::Success ||
cholesky.isNegative() ||
(cholesky.vectorD().array() <= CONSTRAINT_TOLERANCE).any()) {
std::ostringstream message;
message << name << " is not positive definite. "
<< name << "(0,0) is %1%.";
std::string msg(message.str());
return dom_err(function,y(0,0),name,msg.c_str(),"",result);
}
return true;
}
Eigen::Matrix<double, N, N> cholesky(Eigen::Matrix<double, N, N> x) {
Eigen::LDLT<Eigen::Matrix<double, N, N> > ldlt = x.ldlt();
Eigen::Array<double, N, 1> d(ldlt.vectorD().array());
for(int i = 0; i < N; i++) {
if(d[i] < 0) d[i] = 0;
}
Eigen::Matrix<double, N, 1> sqrtd(d.sqrt());
return ldlt.transpositionsP().transpose() * Eigen::Matrix<double, N, N>(ldlt.matrixL()) * sqrtd.asDiagonal();
}
inline T log_determinant_spd(const Eigen::Matrix<T,R,C>& m) {
using std::log;
stan::error_handling::check_square("log_determinant_spd", "m", m);
// Eigen::TriangularView< Eigen::Matrix<T,R,C>, Eigen::Lower > L(m.llt().matrixL());
// T ret(0.0);
// for (size_t i = 0; i < L.rows(); i++)
// ret += log(L(i,i));
// return 2*ret;
return m.ldlt().vectorD().array().log().sum();
}
inline Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>
inv_wishart_rng(const double nu,
const Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic>& S,
RNG& rng) {
Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> S_inv(S.rows(), S.cols());
S_inv = Eigen::MatrixXd::Identity(S.cols(),S.cols());
S_inv = S.ldlt().solve(S_inv);
return wishart_rng(nu, S_inv, rng).inverse();
}
inline Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>
inv_wishart_rng(const double nu,
const Eigen::Matrix
<double, Eigen::Dynamic, Eigen::Dynamic>& S,
RNG& rng) {
static const char* function("stan::math::inv_wishart_rng");
using stan::math::check_greater;
using stan::math::check_square;
using Eigen::MatrixXd;
using stan::math::index_type;
typename index_type<MatrixXd>::type k = S.rows();
check_greater(function, "Degrees of freedom parameter", nu, k-1);
check_square(function, "scale parameter", S);
MatrixXd S_inv = MatrixXd::Identity(k, k);
S_inv = S.ldlt().solve(S_inv);
return wishart_rng(nu, S_inv, rng).inverse();
}
inline Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>
inv_wishart_rng(const double nu,
const Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic>& S,
RNG& rng) {
static const char* function = "stan::prob::inv_wishart_rng(%1%)";
using stan::math::check_greater;
using stan::math::check_size_match;
typename Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic>::size_type k = S.rows();
check_greater(function, nu, k-1, "Degrees of freedom parameter");
check_size_match(function,
S.rows(), "Rows of scale parameter",
S.cols(), "columns of scale parameter");
Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> S_inv(S.rows(), S.cols());
S_inv = Eigen::MatrixXd::Identity(S.cols(),S.cols());
S_inv = S.ldlt().solve(S_inv);
return wishart_rng(nu, S_inv, rng).inverse();
}
void Optimizer::optimizeUseG2O()
{
// create the linear solver
BlockSolverX::LinearSolverType * linearSolver = new LinearSolverCSparse<BlockSolverX::PoseMatrixType>();
// create the block solver on top of the linear solver
BlockSolverX* blockSolver = new BlockSolverX(linearSolver);
// create the algorithm to carry out the optimization
//OptimizationAlgorithmGaussNewton* optimizationAlgorithm = new OptimizationAlgorithmGaussNewton(blockSolver);
OptimizationAlgorithmLevenberg* optimizationAlgorithm = new OptimizationAlgorithmLevenberg(blockSolver);
// NOTE: We skip to fix a variable here, either this is stored in the file
// itself or Levenberg will handle it.
// create the optimizer to load the data and carry out the optimization
SparseOptimizer optimizer;
SparseOptimizer::initMultiThreading();
optimizer.setVerbose(true);
optimizer.setAlgorithm(optimizationAlgorithm);
{
pcl::ScopeTime time("G2O setup Graph vertices");
for (size_t cloud_count = 0; cloud_count < m_pointClouds.size(); ++cloud_count)
{
VertexSE3 *vertex = new VertexSE3;
vertex->setId(cloud_count);
Isometry3D affine = Isometry3D::Identity();
affine.linear() = m_pointClouds[cloud_count]->sensor_orientation_.toRotationMatrix().cast<Isometry3D::Scalar>();
affine.translation() = m_pointClouds[cloud_count]->sensor_origin_.block<3, 1>(0, 0).cast<Isometry3D::Scalar>();
vertex->setEstimate(affine);
optimizer.addVertex(vertex);
}
optimizer.vertex(0)->setFixed(true);
}
{
pcl::ScopeTime time("G2O setup Graph edges");
double trans_noise = 0.5, rot_noise = 0.5235;
EdgeSE3::InformationType infomation = EdgeSE3::InformationType::Zero();
infomation.block<3, 3>(0, 0) << trans_noise * trans_noise, 0, 0,
0, trans_noise * trans_noise, 0,
0, 0, trans_noise * trans_noise;
infomation.block<3, 3>(3, 3) << rot_noise * rot_noise, 0, 0,
0, rot_noise * rot_noise, 0,
0, 0, rot_noise * rot_noise;
for (size_t pair_count = 0; pair_count < m_cloudPairs.size(); ++pair_count)
{
CloudPair pair = m_cloudPairs[pair_count];
int from = pair.corresIdx.first;
int to = pair.corresIdx.second;
EdgeSE3 *edge = new EdgeSE3;
edge->vertices()[0] = optimizer.vertex(from);
edge->vertices()[1] = optimizer.vertex(to);
Eigen::Matrix<double, 6, 6> ATA = Eigen::Matrix<double, 6, 6>::Zero();
Eigen::Matrix<double, 6, 1> ATb = Eigen::Matrix<double, 6, 1>::Zero();
#pragma unroll 8
for (size_t point_count = 0; point_count < pair.corresPointIdx.size(); ++point_count) {
int point_p = pair.corresPointIdx[point_count].first;
int point_q = pair.corresPointIdx[point_count].second;
PointType P = m_pointClouds[from]->points[point_p];
PointType Q = m_pointClouds[to]->points[point_q];
Eigen::Vector3d p = P.getVector3fMap().cast<double>();
Eigen::Vector3d q = Q.getVector3fMap().cast<double>();
Eigen::Vector3d Np = P.getNormalVector3fMap().cast<double>();
double b = (p - q).dot(Np);
Eigen::Matrix<double, 6, 1> A_p;
A_p.block<3, 1>(0, 0) = p.cross(Np);
A_p.block<3, 1>(3, 0) = Np;
ATA += A_p * A_p.transpose();
ATb += A_p * b;
}
Eigen::Matrix<double, 6, 1> X = ATA.ldlt().solve(ATb);
Isometry3D measure = Isometry3D::Identity();
float beta = X[0];
float gammar = X[1];
float alpha = X[2];
measure.linear() = (Eigen::Matrix3d)Eigen::AngleAxisd(alpha, Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(gammar, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(beta, Eigen::Vector3d::UnitX());
measure.translation() = X.block<3, 1>(3, 0);
edge->setMeasurement(measure);
edge->setInformation(infomation);
optimizer.addEdge(edge);
}
}
optimizer.save("debug_preOpt.g2o");
{
pcl::ScopeTime time("g2o optimizing");
optimizer.initializeOptimization();
optimizer.optimize(30);
}
optimizer.save("debug_postOpt.g2o");
for (size_t cloud_count = 0; cloud_count < m_pointClouds.size(); ++cloud_count)
{
CloudTypePtr tmp(new CloudType);
Isometry3D trans = ((VertexSE3 *)optimizer.vertices()[cloud_count])->estimate();
Eigen::Affine3d affine;
affine.linear() = trans.rotation();
affine.translation() = trans.translation();
pcl::transformPointCloudWithNormals(*m_pointClouds[cloud_count], *tmp, affine.cast<float>());
pcl::copyPointCloud(*tmp, *m_pointClouds[cloud_count]);
}
PCL_WARN("Opitimization DONE!!!!\n");
if (m_params.saveDirectory.length()) {
if (boost::filesystem::exists(m_params.saveDirectory) && !boost::filesystem::is_directory(m_params.saveDirectory)) {
boost::filesystem::remove(m_params.saveDirectory);
}
boost::filesystem::create_directories(m_params.saveDirectory);
char filename[1024] = { 0 };
for (size_t i = 0; i < m_pointClouds.size(); ++i) {
sprintf(filename, "%s/cloud_%d.ply", m_params.saveDirectory.c_str(), i);
pcl::io::savePLYFileBinary(filename, *m_pointClouds[i]);
}
}
}
void RGBDOdometry::getIncrementalTransformation(Eigen::Vector3f & trans,
Eigen::Matrix<float, 3, 3, Eigen::RowMajor> & rot,
const bool & rgbOnly,
const float & icpWeight,
const bool & pyramid,
const bool & fastOdom,
const bool & so3)
{
bool icp = !rgbOnly && icpWeight > 0;
bool rgb = rgbOnly || icpWeight < 100;
Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Rprev = rot;
Eigen::Vector3f tprev = trans;
Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Rcurr = Rprev;
Eigen::Vector3f tcurr = tprev;
if(rgb)
{
for(int i = 0; i < NUM_PYRS; i++)
{
computeDerivativeImages(nextImage[i], nextdIdx[i], nextdIdy[i]);
}
}
Eigen::Matrix<double, 3, 3, Eigen::RowMajor> resultR = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>::Identity();
if(so3)
{
int pyramidLevel = 2;
Eigen::Matrix<float, 3, 3, Eigen::RowMajor> R_lr = Eigen::Matrix<float, 3, 3, Eigen::RowMajor>::Identity();
Eigen::Matrix<double, 3, 3, Eigen::RowMajor> K = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>::Zero();
K(0, 0) = intr(pyramidLevel).fx;
K(1, 1) = intr(pyramidLevel).fy;
K(0, 2) = intr(pyramidLevel).cx;
K(1, 2) = intr(pyramidLevel).cy;
K(2, 2) = 1;
float lastError = std::numeric_limits<float>::max() / 2;
float lastCount = std::numeric_limits<float>::max() / 2;
Eigen::Matrix<double, 3, 3, Eigen::RowMajor> lastResultR = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>::Identity();
for(int i = 0; i < 10; i++)
{
Eigen::Matrix<float, 3, 3, Eigen::RowMajor> jtj;
Eigen::Matrix<float, 3, 1> jtr;
Eigen::Matrix<double, 3, 3, Eigen::RowMajor> homography = K * resultR * K.inverse();
mat33 imageBasis;
memcpy(&imageBasis.data[0], homography.cast<float>().eval().data(), sizeof(mat33));
Eigen::Matrix<double, 3, 3, Eigen::RowMajor> K_inv = K.inverse();
mat33 kinv;
memcpy(&kinv.data[0], K_inv.cast<float>().eval().data(), sizeof(mat33));
Eigen::Matrix<double, 3, 3, Eigen::RowMajor> K_R_lr = K * resultR;
mat33 krlr;
memcpy(&krlr.data[0], K_R_lr.cast<float>().eval().data(), sizeof(mat33));
float residual[2];
TICK("so3Step");
so3Step(lastNextImage[pyramidLevel],
nextImage[pyramidLevel],
imageBasis,
kinv,
krlr,
sumDataSO3,
outDataSO3,
jtj.data(),
jtr.data(),
&residual[0],
GPUConfig::getInstance().so3StepThreads,
GPUConfig::getInstance().so3StepBlocks);
TOCK("so3Step");
lastSO3Error = sqrt(residual[0]) / residual[1];
lastSO3Count = residual[1];
//Converged
if(lastSO3Error < lastError && lastCount == lastSO3Count)
{
break;
}
else if(lastSO3Error > lastError + 0.001) //Diverging
{
lastSO3Error = lastError;
lastSO3Count = lastCount;
resultR = lastResultR;
break;
}
lastError = lastSO3Error;
lastCount = lastSO3Count;
lastResultR = resultR;
Eigen::Vector3f delta = jtj.ldlt().solve(jtr);
Eigen::Matrix<double, 3, 3, Eigen::RowMajor> rotUpdate = OdometryProvider::rodrigues(delta.cast<double>());
R_lr = rotUpdate.cast<float>() * R_lr;
for(int x = 0; x < 3; x++)
{
for(int y = 0; y < 3; y++)
{
resultR(x, y) = R_lr(x, y);
}
}
}
}
iterations[0] = fastOdom ? 3 : 10;
iterations[1] = pyramid ? 5 : 0;
iterations[2] = pyramid ? 4 : 0;
Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Rprev_inv = Rprev.inverse();
mat33 device_Rprev_inv = Rprev_inv;
float3 device_tprev = *reinterpret_cast<float3*>(tprev.data());
Eigen::Matrix<double, 4, 4, Eigen::RowMajor> resultRt = Eigen::Matrix<double, 4, 4, Eigen::RowMajor>::Identity();
if(so3)
{
for(int x = 0; x < 3; x++)
{
for(int y = 0; y < 3; y++)
{
resultRt(x, y) = resultR(x, y);
}
}
}
for(int i = NUM_PYRS - 1; i >= 0; i--)
{
if(rgb)
{
projectToPointCloud(lastDepth[i], pointClouds[i], intr, i);
}
Eigen::Matrix<double, 3, 3, Eigen::RowMajor> K = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>::Zero();
K(0, 0) = intr(i).fx;
K(1, 1) = intr(i).fy;
K(0, 2) = intr(i).cx;
K(1, 2) = intr(i).cy;
K(2, 2) = 1;
lastRGBError = std::numeric_limits<float>::max();
for(int j = 0; j < iterations[i]; j++)
{
Eigen::Matrix<double, 4, 4, Eigen::RowMajor> Rt = resultRt.inverse();
Eigen::Matrix<double, 3, 3, Eigen::RowMajor> R = Rt.topLeftCorner(3, 3);
Eigen::Matrix<double, 3, 3, Eigen::RowMajor> KRK_inv = K * R * K.inverse();
mat33 krkInv;
memcpy(&krkInv.data[0], KRK_inv.cast<float>().eval().data(), sizeof(mat33));
Eigen::Vector3d Kt = Rt.topRightCorner(3, 1);
Kt = K * Kt;
float3 kt = {(float)Kt(0), (float)Kt(1), (float)Kt(2)};
int sigma = 0;
int rgbSize = 0;
if(rgb)
{
TICK("computeRgbResidual");
computeRgbResidual(pow(minimumGradientMagnitudes[i], 2.0) / pow(sobelScale, 2.0),
nextdIdx[i],
nextdIdy[i],
lastDepth[i],
nextDepth[i],
lastImage[i],
nextImage[i],
corresImg[i],
sumResidualRGB,
maxDepthDeltaRGB,
kt,
krkInv,
sigma,
rgbSize,
GPUConfig::getInstance().rgbResThreads,
GPUConfig::getInstance().rgbResBlocks);
TOCK("computeRgbResidual");
}
float sigmaVal = std::sqrt((float)sigma / rgbSize == 0 ? 1 : rgbSize);
float rgbError = std::sqrt(sigma) / (rgbSize == 0 ? 1 : rgbSize);
if(rgbOnly && rgbError > lastRGBError)
{
break;
}
lastRGBError = rgbError;
lastRGBCount = rgbSize;
if(rgbOnly)
{
sigmaVal = -1; //Signals the internal optimisation to weight evenly
}
Eigen::Matrix<float, 6, 6, Eigen::RowMajor> A_icp;
Eigen::Matrix<float, 6, 1> b_icp;
mat33 device_Rcurr = Rcurr;
float3 device_tcurr = *reinterpret_cast<float3*>(tcurr.data());
DeviceArray2D<float>& vmap_curr = vmaps_curr_[i];
DeviceArray2D<float>& nmap_curr = nmaps_curr_[i];
DeviceArray2D<float>& vmap_g_prev = vmaps_g_prev_[i];
DeviceArray2D<float>& nmap_g_prev = nmaps_g_prev_[i];
float residual[2];
if(icp)
{
TICK("icpStep");
icpStep(device_Rcurr,
device_tcurr,
vmap_curr,
nmap_curr,
device_Rprev_inv,
device_tprev,
intr(i),
vmap_g_prev,
nmap_g_prev,
distThres_,
angleThres_,
sumDataSE3,
outDataSE3,
A_icp.data(),
b_icp.data(),
&residual[0],
GPUConfig::getInstance().icpStepThreads,
GPUConfig::getInstance().icpStepBlocks);
TOCK("icpStep");
}
lastICPError = sqrt(residual[0]) / residual[1];
lastICPCount = residual[1];
Eigen::Matrix<float, 6, 6, Eigen::RowMajor> A_rgbd;
Eigen::Matrix<float, 6, 1> b_rgbd;
if(rgb)
{
TICK("rgbStep");
rgbStep(corresImg[i],
sigmaVal,
pointClouds[i],
intr(i).fx,
intr(i).fy,
nextdIdx[i],
nextdIdy[i],
sobelScale,
sumDataSE3,
outDataSE3,
A_rgbd.data(),
b_rgbd.data(),
GPUConfig::getInstance().rgbStepThreads,
GPUConfig::getInstance().rgbStepBlocks);
TOCK("rgbStep");
}
Eigen::Matrix<double, 6, 1> result;
Eigen::Matrix<double, 6, 6, Eigen::RowMajor> dA_rgbd = A_rgbd.cast<double>();
Eigen::Matrix<double, 6, 6, Eigen::RowMajor> dA_icp = A_icp.cast<double>();
Eigen::Matrix<double, 6, 1> db_rgbd = b_rgbd.cast<double>();
Eigen::Matrix<double, 6, 1> db_icp = b_icp.cast<double>();
if(icp && rgb)
{
double w = icpWeight;
lastA = dA_rgbd + w * w * dA_icp;
lastb = db_rgbd + w * db_icp;
result = lastA.ldlt().solve(lastb);
}
else if(icp)
{
lastA = dA_icp;
lastb = db_icp;
result = lastA.ldlt().solve(lastb);
}
else if(rgb)
{
lastA = dA_rgbd;
lastb = db_rgbd;
result = lastA.ldlt().solve(lastb);
}
else
{
assert(false && "Control shouldn't reach here");
}
Eigen::Isometry3f rgbOdom;
OdometryProvider::computeUpdateSE3(resultRt, result, rgbOdom);
Eigen::Isometry3f currentT;
currentT.setIdentity();
currentT.rotate(Rprev);
currentT.translation() = tprev;
currentT = currentT * rgbOdom.inverse();
tcurr = currentT.translation();
Rcurr = currentT.rotation();
}
}
if(rgb && (tcurr - tprev).norm() > 0.3)
{
Rcurr = Rprev;
tcurr = tprev;
}
if(so3)
{
for(int i = 0; i < NUM_PYRS; i++)
{
std::swap(lastNextImage[i], nextImage[i]);
}
}
trans = tcurr;
rot = Rcurr;
}
bool CameraModel::_lift( const Eigen::MatrixBase<Derived1>& mP2D,
Eigen::MatrixBase<Derived2>& mP3D ) const {
typedef typename Eigen::internal::traits<Derived2>::Scalar ScalarType;
const unsigned int nNumPoints = mP2D.cols();
#if 0
typedef Eigen::Matrix<ScalarType,1,Eigen::Dynamic> RowVect;
const int nWidth = 1000;
const int nHeight = 1000;
const int nWidthSize = nWidth/10;
const int nHeightSize = nHeight/10;
Derived1 mP2DUndist( 3, nWidthSize*nHeightSize );
Derived1 m1 = RowVect::LinSpaced( nWidthSize, 0, nWidth-1 ).replicate( nHeightSize, 1 );
mP2DUndist.row( 0 ) = Eigen::Map<Derived1>( m1.data(), 1, nWidthSize*nHeightSize );
mP2DUndist.row( 1 ) = RowVect::LinSpaced( nHeightSize, 0, nHeight-1 ).replicate( 1, nWidthSize );
mP2DUndist.row( 2 ) = RowVect::Ones( 1, nWidthSize*nHeightSize );
Derived1 mP2DDist( 2, nWidthSize*nHeightSize );
project( mP2DUndist, mP2DDist );
// Initialise with closest values
for( int ii=0; ii<mP2D.cols(); ii++ ) {
//Derived1::Index nMinIndex;
int nMinIndex;
Derived1 mDiff = mP2DDist;
mDiff.colwise() -= mP2D.col( ii );
mDiff.colwise().norm().minCoeff( &nMinIndex );
mP3D.col( ii ) = mP2DUndist.col( nMinIndex );
}
#else
// Start from distortionless points if possible
if( get( "fx" ) != "" && get( "fy" ) != "" &&
get( "cx" ) != "" && get( "cy" ) != "" ) {
double fx = get<double>( "fx" ); double fy = get<double>( "fy" );
double cx = get<double>( "cx" ); double cy = get<double>( "cy" );
mP3D.row( 0 ) = ( mP2D.row( 0 ) - cx*Derived2::Ones( 1, nNumPoints ) ) / fx;
mP3D.row( 1 ) = ( mP2D.row( 1 ) - cy*Derived2::Ones( 1, nNumPoints ) ) / fy;
mP3D.row( 2 ) = Derived2::Ones( 1, nNumPoints );
}
else {
mP3D.row( 0 ) = mP2D.row( 0 );
mP3D.row( 1 ) = mP2D.row( 1 );
mP3D.row( 2 ) = Derived2::Ones( 1, nNumPoints );
}
#endif
for( size_t nIndex=0; nIndex<nNumPoints; nIndex++ ) {
mP3D.col( nIndex ) /= mP3D.col( nIndex ).norm();
}
Derived1 mP2DEst( 2, nNumPoints );
Derived2 mdP2DE( 2, 3*nNumPoints );
Derived2 mdP2DI( 2, get_num_parameters()*nNumPoints );
double dMaxErr = 0.001;
double dLastMaxErr = 10;
const unsigned int nMaxIter = 30;
const unsigned int nMaxOuterIter = 5;
for( size_t nOuterIter=0; nOuterIter<nMaxOuterIter; nOuterIter++ ) {
for( size_t nIter=0; nIter<nMaxIter; nIter++ ) {
project( mP3D, mP2DEst, mdP2DE, mdP2DI );
for( size_t nIndex=0; nIndex<nNumPoints; nIndex++ ) {
Eigen::Matrix<ScalarType,2,3> mJ = mdP2DE.block( 0, 3*nIndex, 2, 3 );
Eigen::Matrix<ScalarType,3,3> mJtJ = mJ.transpose()*mJ + 0.1*Eigen::Matrix<ScalarType,3,3>::Identity();
Eigen::Matrix<ScalarType,3,1> mJte = mJ.transpose() * ( mP2DEst.col( nIndex ) - mP2D.col( nIndex ) );
mP3D.col( nIndex ) -= mJtJ.ldlt().solve( mJte );
double dErr = ( mP2DEst.col( nIndex ) - mP2D.col( nIndex ) ).norm();
if( nIndex > 0 ) {
if( std::isnan( dErr ) ) {
mP3D.col( nIndex ) = mP3D.col( nIndex-1 );
}
}
mP3D.col( nIndex ) /= mP3D.col( nIndex ).norm();
}
dLastMaxErr = ( mP2DEst - mP2D ).colwise().norm().maxCoeff();
if( dLastMaxErr < dMaxErr ) {
break;
}
}
if( dLastMaxErr >= dMaxErr ) {
Derived1 mErrors = ( mP2DEst - mP2D ).colwise().norm();
for( int ii=0; ii<mErrors.cols(); ii++ ) {
if( mErrors(0,ii) > dMaxErr ) {
// Find closest value
double dMinDiff = 1e10;
int nBestIndex = -1;
for( int jj=0; jj<mErrors.cols(); jj++ ) {
if( jj != ii && mErrors(0,jj) < dMaxErr ) {
double dDiff = ( mP2D.col( ii ) - mP2D.col( jj ) ).norm();
if( dDiff < dMinDiff ) {
dMinDiff = dDiff;
nBestIndex = jj;
}
}
}
if( nBestIndex == -1 ) {
// All is lost, could not find an index
// that passes the error test
return false;
}
mP3D.col( ii ) = mP3D.col( nBestIndex ) ;
}
}
}
}
return dLastMaxErr < dMaxErr;
}