static bool testApply()
{
bool b, ret;
// apply delta:
Eigen::Matrix<double, 6, 1> delta = Eigen::Matrix<double, 6, 1>::Zero();
Eigen::Matrix<double, 4, 4> expectedT = Eigen::Matrix<double, 4, 4>::Identity();
Eigen::Matrix<double, 4, 4> diff;
SE3<double> pose;
pose.set( delta );
delta[ 0 ] = Math::deg2Rad( 1.5 );
delta[ 1 ] = Math::deg2Rad( 1.1 );
delta[ 2 ] = Math::deg2Rad( 1.6 );
delta[ 3 ] = 1;
delta[ 4 ] = 1;
delta[ 5 ] = 1;
pose.apply( delta );
expectedT( 0, 3 ) = delta[ 3 ];
expectedT( 1, 3 ) = delta[ 4 ];
expectedT( 2, 3 ) = delta[ 5 ];
Eigen::Matrix<double, 3, 1> axis = delta.segment<3>( 0 );
double angle = axis.norm(); axis /= angle;
expectedT.block<3, 3>( 0, 0 ) = Eigen::AngleAxis<double>( angle, axis ).toRotationMatrix();
diff = expectedT - pose.transformation();
ret = b = ( diff.array().abs().sum() / 12 < 0.001 );
if( !b ){
std::cout << expectedT << std::endl;
std::cout << pose.transformation() << std::endl;
std::cout << "avg SAD: " << diff.array().abs().sum() / 12 << std::endl;
}
pose.apply( -delta );
expectedT.setIdentity();
b &= ( ( expectedT - pose.transformation() ).array().abs().sum() / 12 < 0.0001 );
CVTTEST_PRINT( "apply", b );
ret &= b;
return ret;
}
IGL_INLINE void igl::repdiag(
const Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> & A,
const int d,
Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> & B)
{
int m = A.rows();
int n = A.cols();
B.resize(m*d,n*d);
B.array() *= 0;
for(int i = 0;i<d;i++)
{
B.block(i*m,i*n,m,n) = A;
}
}
void SignedDistanceField::calculateSignedDistanceField(const GridMap& gridMap, const std::string& layer,
const double heightClearance)
{
data_.clear();
resolution_ = gridMap.getResolution();
position_ = gridMap.getPosition();
size_ = gridMap.getSize();
Matrix map = gridMap.get(layer); // Copy!
float minHeight = map.minCoeffOfFinites();
if (!std::isfinite(minHeight)) minHeight = lowestHeight_;
float maxHeight = map.maxCoeffOfFinites();
if (!std::isfinite(maxHeight)) maxHeight = lowestHeight_;
const float valueForEmptyCells = lowestHeight_; // maxHeight, minHeight (TODO Make this an option).
for (size_t i = 0; i < map.size(); ++i) {
if (std::isnan(map(i))) map(i) = valueForEmptyCells;
}
// Height range of the signed distance field is higher than the max height.
maxHeight += heightClearance;
Matrix sdfElevationAbove = Matrix::Ones(map.rows(), map.cols()) * maxDistance_;
Matrix sdfLayer = Matrix::Zero(map.rows(), map.cols());
std::vector<Matrix> sdf;
zIndexStartHeight_ = minHeight;
// Calculate signed distance field from bottom.
for (float h = minHeight; h < maxHeight; h += resolution_) {
Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> obstacleFreeField = map.array() < h;
Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> obstacleField = obstacleFreeField.array() < 1;
Matrix sdfObstacle = getPlanarSignedDistanceField(obstacleField);
Matrix sdfObstacleFree = getPlanarSignedDistanceField(obstacleFreeField);
Matrix sdf2d;
// If 2d sdfObstacleFree calculation failed, neglect this SDF
// to avoid extreme small distances (-INF).
if ((sdfObstacleFree.array() >= INF).any()) sdf2d = sdfObstacle;
else sdf2d = sdfObstacle - sdfObstacleFree;
sdf2d *= resolution_;
for (size_t i = 0; i < sdfElevationAbove.size(); ++i) {
if(sdfElevationAbove(i) == maxDistance_ && map(i) <= h) sdfElevationAbove(i) = h - map(i);
else if(sdfElevationAbove(i) != maxDistance_ && map(i) <= h) sdfElevationAbove(i) = sdfLayer(i) + resolution_;
if (sdf2d(i) == 0) sdfLayer(i) = h - map(i);
else if (sdf2d(i) < 0) sdfLayer(i) = -std::min(fabs(sdf2d(i)), fabs(map(i) - h));
else sdfLayer(i) = std::min(sdf2d(i), sdfElevationAbove(i));
}
data_.push_back(sdfLayer);
}
}
IGL_INLINE Eigen::Matrix<Scalar,3,1> igl::unproject(
const Eigen::Matrix<Scalar,3,1>& win,
const Eigen::Matrix<Scalar,4,4>& model,
const Eigen::Matrix<Scalar,4,4>& proj,
const Eigen::Matrix<Scalar,4,1>& viewport)
{
Eigen::Matrix<Scalar,4,4> Inverse = (proj * model).inverse();
Eigen::Matrix<Scalar,4,1> tmp;
tmp << win, 1;
tmp(0) = (tmp(0) - viewport(0)) / viewport(2);
tmp(1) = (tmp(1) - viewport(1)) / viewport(3);
tmp = tmp.array() * 2.0f - 1.0f;
Eigen::Matrix<Scalar,4,1> obj = Inverse * tmp;
obj /= obj(3);
return obj.head(3);
}
Eigen::Matrix<Scalar,3,1> igl::project(
const Eigen::Matrix<Scalar,3,1>& obj,
const Eigen::Matrix<Scalar,4,4>& model,
const Eigen::Matrix<Scalar,4,4>& proj,
const Eigen::Matrix<Scalar,4,1>& viewport)
{
Eigen::Matrix<Scalar,4,1> tmp;
tmp << obj,1;
tmp = model * tmp;
tmp = proj * tmp;
tmp = tmp.array() / tmp(3);
tmp = tmp.array() * 0.5f + 0.5f;
tmp(0) = tmp(0) * viewport(2) + viewport(0);
tmp(1) = tmp(1) * viewport(3) + viewport(1);
return tmp.head(3);
}
PointCoordinateMatrices ConverterPlaneFromTo3d::projectTo3d(Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> image)
{
PointCoordinateMatrices reshapedCoordinates;
//std::cout << "image: " << std::endl << image << std::endl << std::endl;
//As defined for the example data sets to get the distance in meters Z = depth / 5000 TODO change for kinect data (maybe is the same...)
Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> z = image.array() / 1;// 5000;
Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> x = z.cwiseProduct(xAdjustment_);
Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> y = z.cwiseProduct(yAdjustment_);
//std::cout << "x: " << std::endl << x << std::endl << std::endl;
//std::cout << "y: " << std::endl << y << std::endl << std::endl;
//std::cout << "z: " << std::endl << z << std::endl << std::endl;
reshapedCoordinates.x = Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(x.data(), 1, image.size());
reshapedCoordinates.y = Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(y.data(), 1, image.size());
reshapedCoordinates.z = Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(z.data(), 1, image.size());
return reshapedCoordinates;
}
static bool testHessian()
{
Eigen::Matrix<double, 6, 1> delta = Eigen::Matrix<double, 6, 1>::Zero();
Eigen::Matrix<double, 24, 6> hN, hA;
SE3<double> pose;
pose.set( Math::deg2Rad( 10.0 ), Math::deg2Rad( 40.0 ), Math::deg2Rad( -120.0 ), -100.0, 200.0, 300.0 );
Eigen::Matrix<double, 3, 3> K( Eigen::Matrix<double, 3, 3>::Zero() );
K( 0, 0 ) = 650.0; K( 0, 2 ) = 320.0;
K( 1, 1 ) = 650.0; K( 1, 2 ) = 240.0;
K( 2, 2 ) = 1.0;
Eigen::Matrix<double, 3, 1> point;
Eigen::Matrix<double, 3, 1> p, ff, fb, bf, bb, xxf, xxb, hess;
point[ 0 ] = 16;
point[ 1 ] = 80;
point[ 2 ] = 13;
pose.transform( p, point );
double h = 0.0001;
for( size_t i = 0; i < 6; i++ ){
for( size_t j = 0; j < 6; j++ ){
delta.setZero();
if( i == j ){
// +
delta[ j ] = h;
pose.apply( delta );
pose.transform( xxf, point );
pose.apply( -delta );
delta[ j ] = -h;
pose.apply( delta );
pose.transform( xxb, point );
pose.apply( -delta );
hess = ( xxb - 2 * p + xxf ) / ( h*h );
} else {
delta[ i ] = h;
delta[ j ] = h;
pose.apply( delta );
pose.transform( ff, point );
pose.apply( -delta );
delta[ i ] = h;
delta[ j ] = -h;
pose.apply( delta );
pose.transform( fb, point );
pose.apply( -delta );
delta[ i ] = -h;
delta[ j ] = h;
pose.apply( delta );
pose.transform( bf, point );
pose.apply( -delta );
delta[ i ] = -h;
delta[ j ] = -h;
pose.apply( delta );
pose.transform( bb, point );
pose.apply( -delta );
hess = ( ff - bf - fb + bb ) / ( 4 * h * h );
}
hN( 4 * i , j ) = hess[ 0 ];
hN( 4 * i + 1 , j ) = hess[ 1 ];
hN( 4 * i + 2 , j ) = hess[ 2 ];
hN( 4 * i + 3 , j ) = 0.0;
}
}
pose.hessian( hA, p );
bool b, ret = true;
Eigen::Matrix<double, 24, 6> jDiff;
jDiff = hN - hA;
b = ( jDiff.array().abs().sum() / ( double )( jDiff.rows() * jDiff.cols() ) ) < 0.00001;
CVTTEST_PRINT( "Pose Hessian", b );
if( !b ){
std::cout << "Analytic:\n" << hA << std::endl;
std::cout << "Numeric:\n" << hN << std::endl;
std::cout << "Difference:\n" << jDiff << std::endl;
}
ret &= b;
return ret;
}
static bool testScreenHessian()
{
Eigen::Matrix<double, 6, 1> delta = Eigen::Matrix<double, 6, 1>::Zero();
Eigen::Matrix<double, 6, 6> shNumericX, shNumericY, shX, shY;
SE3<double> pose;
pose.set( Math::deg2Rad( 10.0 ), Math::deg2Rad( 40.0 ), Math::deg2Rad( -120.0 ), -100.0, 200.0, 300.0 );
Eigen::Matrix<double, 3, 3> K( Eigen::Matrix<double, 3, 3>::Zero() );
K( 0, 0 ) = 650.0; K( 0, 2 ) = 320.0;
K( 1, 1 ) = 650.0; K( 1, 2 ) = 240.0;
K( 2, 2 ) = 1.0;
Eigen::Matrix<double, 3, 1> point, ptrans;
Eigen::Matrix<double, 2, 1> sp, ff, fb, bf, bb, xxf, xxb, hess;
point[ 0 ] = 100; point[ 1 ] = 200; point[ 2 ] = 300;
// project the point with current parameters
pose.transform( ptrans, point );
projectWithCam( sp, ptrans, K );
double h = 0.001;
for( size_t i = 0; i < 6; i++ ){
for( size_t j = 0; j < 6; j++ ){
if( i == j ){
// +
delta[ j ] = h;
pose.apply( delta );
pose.transform( ptrans, point );
projectWithCam( xxf, ptrans, K );
delta[ j ] = -2 * h;
pose.apply( delta );
pose.transform( ptrans, point );
projectWithCam( xxb, ptrans, K );
hess = ( xxb - 2 * sp + xxf ) / ( h*h );
// back to start
delta[ j ] = h;
pose.apply( delta );
delta[ j ] = 0;
} else {
delta[ i ] = h;
delta[ j ] = h;
pose.apply( delta );
pose.transform( ptrans, point );
projectWithCam( ff, ptrans, K );
pose.apply( -delta );
delta[ i ] = h;
delta[ j ] = -h;
pose.apply( delta );
pose.transform( ptrans, point );
projectWithCam( fb, ptrans, K );
pose.apply( -delta );
delta[ i ] = -h;
delta[ j ] = h;
pose.apply( delta );
pose.transform( ptrans, point );
projectWithCam( bf, ptrans, K );
pose.apply( -delta );
delta[ i ] = -h;
delta[ j ] = -h;
pose.apply( delta );
pose.transform( ptrans, point );
projectWithCam( bb, ptrans, K );
pose.apply( -delta );
hess = ( ff - bf - fb + bb ) / ( 4 * h * h );
delta.setZero();
}
shNumericX( i, j ) = hess[ 0 ];
shNumericY( i, j ) = hess[ 1 ];
}
}
pose.transform( ptrans, point );
pose.screenHessian( shX, shY, ptrans, K );
bool b, ret = true;
Eigen::Matrix<double, 6, 6> jDiff;
jDiff = shNumericX - shX;
b = ( jDiff.array().abs().sum() / ( double )( jDiff.rows() * jDiff.cols() ) ) < 0.0001;
CVTTEST_PRINT( "Pose ScreenHessian X", b );
if( !b ){
std::cout << "Analytic:\n" << shX << std::endl;
std::cout << "Numeric:\n" << shNumericX << std::endl;
std::cout << "Difference:\n" << jDiff << std::endl;
}
ret &= b;
jDiff = shNumericY - shY;
b = ( jDiff.array().abs().sum() / ( double )( jDiff.rows() * jDiff.cols() ) ) < 0.0001;
CVTTEST_PRINT( "Pose ScreenHessian Y", b );
if( !b ){
std::cout << "Analytic:\n" << shY << std::endl;
std::cout << "Numeric:\n" << shNumericY << std::endl;
std::cout << "Difference:\n" << jDiff << std::endl;
}
ret &= b;
return ret;
}
static bool testScreenJacobian()
{
Eigen::Matrix<double, 6, 1> delta = Eigen::Matrix<double, 6, 1>::Zero();
Eigen::Matrix<double, 2, 6> shNumeric, sh;
SE3<double> pose;
pose.set( Math::deg2Rad( 10.0 ), Math::deg2Rad( 40.0 ), Math::deg2Rad( -120.0 ), -100.0, 200.0, 300.0 );
Eigen::Matrix<double, 3, 3> K( Eigen::Matrix<double, 3, 3>::Zero() );
K( 0, 0 ) = 650.0; K( 0, 2 ) = 320.0;
K( 1, 1 ) = 650.0; K( 1, 2 ) = 240.0;
K( 2, 2 ) = 1.0;
Eigen::Matrix<double, 3, 1> point, ptrans;
Eigen::Matrix<double, 2, 1> sp, ff, bb, jac;
point[ 0 ] = 100; point[ 1 ] = 200; point[ 2 ] = 300;
// project the point with current parameters
pose.transform( ptrans, point );
projectWithCam( sp, ptrans, K );
double h = 0.001;
for( size_t i = 0; i < 6; i++ ){
delta[ i ] = h;
pose.apply( delta );
pose.transform( ptrans, point );
projectWithCam( ff, ptrans, K );
pose.apply( -delta );
delta[ i ] = -h;
pose.apply( delta );
pose.transform( ptrans, point );
projectWithCam( bb, ptrans, K );
pose.apply( -delta );
jac = ( ff - bb ) / ( 2 * h );
delta.setZero();
shNumeric( 0, i ) = jac[ 0 ];
shNumeric( 1, i ) = jac[ 1 ];
}
pose.transform( ptrans, point );
pose.screenJacobian( sh, ptrans, K );
bool b, ret = true;
Eigen::Matrix<double, 2, 6> jDiff;
jDiff = shNumeric - sh;
b = ( jDiff.array().abs().sum() / ( double )( jDiff.rows() * jDiff.cols() ) ) < 0.0001;
CVTTEST_PRINT( "Pose ScreenJacobian", b );
if( !b ){
std::cout << "Analytic:\n" << sh << std::endl;
std::cout << "Numeric:\n" << shNumeric << std::endl;
std::cout << "Difference:\n" << jDiff << std::endl;
}
ret &= b;
return ret;
}
IGL_INLINE igl::SolverStatus igl::active_set(
const Eigen::SparseMatrix<AT>& A,
const Eigen::PlainObjectBase<DerivedB> & B,
const Eigen::PlainObjectBase<Derivedknown> & known,
const Eigen::PlainObjectBase<DerivedY> & Y,
const Eigen::SparseMatrix<AeqT>& Aeq,
const Eigen::PlainObjectBase<DerivedBeq> & Beq,
const Eigen::SparseMatrix<AieqT>& Aieq,
const Eigen::PlainObjectBase<DerivedBieq> & Bieq,
const Eigen::PlainObjectBase<Derivedlx> & p_lx,
const Eigen::PlainObjectBase<Derivedux> & p_ux,
const igl::active_set_params & params,
Eigen::PlainObjectBase<DerivedZ> & Z
)
{
//#define ACTIVE_SET_CPP_DEBUG
#ifdef ACTIVE_SET_CPP_DEBUG
# warning "ACTIVE_SET_CPP_DEBUG"
#endif
using namespace Eigen;
using namespace std;
SolverStatus ret = SOLVER_STATUS_ERROR;
const int n = A.rows();
assert(n == A.cols() && "A must be square");
// Discard const qualifiers
//if(B.size() == 0)
//{
// B = Eigen::PlainObjectBase<DerivedB>::Zero(n,1);
//}
assert(n == B.rows() && "B.rows() must match A.rows()");
assert(B.cols() == 1 && "B must be a column vector");
assert(Y.cols() == 1 && "Y must be a column vector");
assert((Aeq.size() == 0 && Beq.size() == 0) || Aeq.cols() == n);
assert((Aeq.size() == 0 && Beq.size() == 0) || Aeq.rows() == Beq.rows());
assert((Aeq.size() == 0 && Beq.size() == 0) || Beq.cols() == 1);
assert((Aieq.size() == 0 && Bieq.size() == 0) || Aieq.cols() == n);
assert((Aieq.size() == 0 && Bieq.size() == 0) || Aieq.rows() == Bieq.rows());
assert((Aieq.size() == 0 && Bieq.size() == 0) || Bieq.cols() == 1);
Eigen::Matrix<typename Derivedlx::Scalar,Eigen::Dynamic,1> lx;
Eigen::Matrix<typename Derivedux::Scalar,Eigen::Dynamic,1> ux;
if(p_lx.size() == 0)
{
lx = Eigen::PlainObjectBase<Derivedlx>::Constant(
n,1,-numeric_limits<typename Derivedlx::Scalar>::max());
}else
{
lx = p_lx;
}
if(ux.size() == 0)
{
ux = Eigen::PlainObjectBase<Derivedux>::Constant(
n,1,numeric_limits<typename Derivedux::Scalar>::max());
}else
{
ux = p_ux;
}
assert(lx.rows() == n && "lx must have n rows");
assert(ux.rows() == n && "ux must have n rows");
assert(ux.cols() == 1 && "lx must be a column vector");
assert(lx.cols() == 1 && "ux must be a column vector");
assert((ux.array()-lx.array()).minCoeff() > 0 && "ux(i) must be > lx(i)");
if(Z.size() != 0)
{
// Initial guess should have correct size
assert(Z.rows() == n && "Z must have n rows");
assert(Z.cols() == 1 && "Z must be a column vector");
}
assert(known.cols() == 1 && "known must be a column vector");
// Number of knowns
const int nk = known.size();
// Initialize active sets
typedef int BOOL;
#define TRUE 1
#define FALSE 0
Matrix<BOOL,Dynamic,1> as_lx = Matrix<BOOL,Dynamic,1>::Constant(n,1,FALSE);
Matrix<BOOL,Dynamic,1> as_ux = Matrix<BOOL,Dynamic,1>::Constant(n,1,FALSE);
Matrix<BOOL,Dynamic,1> as_ieq = Matrix<BOOL,Dynamic,1>::Constant(Aieq.rows(),1,FALSE);
// Keep track of previous Z for comparison
PlainObjectBase<DerivedZ> old_Z;
old_Z = PlainObjectBase<DerivedZ>::Constant(
n,1,numeric_limits<typename DerivedZ::Scalar>::max());
int iter = 0;
while(true)
{
#ifdef ACTIVE_SET_CPP_DEBUG
cout<<"Iteration: "<<iter<<":"<<endl;
cout<<" pre"<<endl;
#endif
// FIND BREACHES OF CONSTRAINTS
int new_as_lx = 0;
int new_as_ux = 0;
int new_as_ieq = 0;
if(Z.size() > 0)
{
for(int z = 0;z < n;z++)
{
if(Z(z) < lx(z))
{
new_as_lx += (as_lx(z)?0:1);
//new_as_lx++;
as_lx(z) = TRUE;
}
if(Z(z) > ux(z))
{
new_as_ux += (as_ux(z)?0:1);
//new_as_ux++;
as_ux(z) = TRUE;
}
}
if(Aieq.rows() > 0)
{
PlainObjectBase<DerivedZ> AieqZ;
AieqZ = Aieq*Z;
for(int a = 0;a<Aieq.rows();a++)
{
if(AieqZ(a) > Bieq(a))
{
new_as_ieq += (as_ieq(a)?0:1);
as_ieq(a) = TRUE;
}
}
}
#ifdef ACTIVE_SET_CPP_DEBUG
cout<<" new_as_lx: "<<new_as_lx<<endl;
cout<<" new_as_ux: "<<new_as_ux<<endl;
#endif
const double diff = (Z-old_Z).squaredNorm();
#ifdef ACTIVE_SET_CPP_DEBUG
cout<<"diff: "<<diff<<endl;
#endif
if(diff < params.solution_diff_threshold)
{
ret = SOLVER_STATUS_CONVERGED;
break;
}
old_Z = Z;
}
const int as_lx_count = count(as_lx.data(),as_lx.data()+n,TRUE);
const int as_ux_count = count(as_ux.data(),as_ux.data()+n,TRUE);
const int as_ieq_count =
count(as_ieq.data(),as_ieq.data()+as_ieq.size(),TRUE);
#ifndef NDEBUG
{
int count = 0;
for(int a = 0;a<as_ieq.size();a++)
{
if(as_ieq(a))
{
assert(as_ieq(a) == TRUE);
count++;
}
}
assert(as_ieq_count == count);
}
#endif
// PREPARE FIXED VALUES
PlainObjectBase<Derivedknown> known_i;
known_i.resize(nk + as_lx_count + as_ux_count,1);
PlainObjectBase<DerivedY> Y_i;
Y_i.resize(nk + as_lx_count + as_ux_count,1);
{
known_i.block(0,0,known.rows(),known.cols()) = known;
Y_i.block(0,0,Y.rows(),Y.cols()) = Y;
int k = nk;
// Then all lx
for(int z = 0;z < n;z++)
{
if(as_lx(z))
{
known_i(k) = z;
Y_i(k) = lx(z);
k++;
}
}
// Finally all ux
for(int z = 0;z < n;z++)
{
if(as_ux(z))
{
known_i(k) = z;
Y_i(k) = ux(z);
k++;
}
}
assert(k==Y_i.size());
assert(k==known_i.size());
}
//cout<<matlab_format((known_i.array()+1).eval(),"known_i")<<endl;
// PREPARE EQUALITY CONSTRAINTS
VectorXi as_ieq_list(as_ieq_count,1);
// Gather active constraints and resp. rhss
PlainObjectBase<DerivedBeq> Beq_i;
Beq_i.resize(Beq.rows()+as_ieq_count,1);
Beq_i.head(Beq.rows()) = Beq;
{
int k =0;
for(int a=0;a<as_ieq.size();a++)
{
if(as_ieq(a))
{
assert(k<as_ieq_list.size());
as_ieq_list(k)=a;
Beq_i(Beq.rows()+k,0) = Bieq(k,0);
k++;
}
}
assert(k == as_ieq_count);
}
// extract active constraint rows
SparseMatrix<AeqT> Aeq_i,Aieq_i;
slice(Aieq,as_ieq_list,1,Aieq_i);
// Append to equality constraints
cat(1,Aeq,Aieq_i,Aeq_i);
min_quad_with_fixed_data<AT> data;
#ifndef NDEBUG
{
// NO DUPES!
Matrix<BOOL,Dynamic,1> fixed = Matrix<BOOL,Dynamic,1>::Constant(n,1,FALSE);
for(int k = 0;k<known_i.size();k++)
{
assert(!fixed[known_i(k)]);
fixed[known_i(k)] = TRUE;
}
}
#endif
Eigen::PlainObjectBase<DerivedZ> sol;
if(known_i.size() == A.rows())
{
// Everything's fixed?
#ifdef ACTIVE_SET_CPP_DEBUG
cout<<" everything's fixed."<<endl;
#endif
Z.resize(A.rows(),Y_i.cols());
slice_into(Y_i,known_i,1,Z);
sol.resize(0,Y_i.cols());
assert(Aeq_i.rows() == 0 && "All fixed but linearly constrained");
}else
{
#ifdef ACTIVE_SET_CPP_DEBUG
cout<<" min_quad_with_fixed_precompute"<<endl;
#endif
if(!min_quad_with_fixed_precompute(A,known_i,Aeq_i,params.Auu_pd,data))
{
cerr<<"Error: min_quad_with_fixed precomputation failed."<<endl;
if(iter > 0 && Aeq_i.rows() > Aeq.rows())
{
cerr<<" *Are you sure rows of [Aeq;Aieq] are linearly independent?*"<<
endl;
}
ret = SOLVER_STATUS_ERROR;
break;
}
#ifdef ACTIVE_SET_CPP_DEBUG
cout<<" min_quad_with_fixed_solve"<<endl;
#endif
if(!min_quad_with_fixed_solve(data,B,Y_i,Beq_i,Z,sol))
{
cerr<<"Error: min_quad_with_fixed solve failed."<<endl;
ret = SOLVER_STATUS_ERROR;
break;
}
//cout<<matlab_format((Aeq*Z-Beq).eval(),"cr")<<endl;
//cout<<matlab_format(Z,"Z")<<endl;
#ifdef ACTIVE_SET_CPP_DEBUG
cout<<" post"<<endl;
#endif
// Computing Lagrange multipliers needs to be adjusted slightly if A is not symmetric
assert(data.Auu_sym);
}
// Compute Lagrange multiplier values for known_i
SparseMatrix<AT> Ak;
// Slow
slice(A,known_i,1,Ak);
Eigen::PlainObjectBase<DerivedB> Bk;
slice(B,known_i,Bk);
MatrixXd Lambda_known_i = -(0.5*Ak*Z + 0.5*Bk);
// reverse the lambda values for lx
Lambda_known_i.block(nk,0,as_lx_count,1) =
(-1*Lambda_known_i.block(nk,0,as_lx_count,1)).eval();
// Extract Lagrange multipliers for Aieq_i (always at back of sol)
VectorXd Lambda_Aieq_i(Aieq_i.rows(),1);
for(int l = 0;l<Aieq_i.rows();l++)
{
Lambda_Aieq_i(Aieq_i.rows()-1-l) = sol(sol.rows()-1-l);
}
// Remove from active set
for(int l = 0;l<as_lx_count;l++)
{
if(Lambda_known_i(nk + l) < params.inactive_threshold)
{
as_lx(known_i(nk + l)) = FALSE;
}
}
for(int u = 0;u<as_ux_count;u++)
{
if(Lambda_known_i(nk + as_lx_count + u) <
params.inactive_threshold)
{
as_ux(known_i(nk + as_lx_count + u)) = FALSE;
}
}
for(int a = 0;a<as_ieq_count;a++)
{
if(Lambda_Aieq_i(a) < params.inactive_threshold)
{
as_ieq(as_ieq_list(a)) = FALSE;
}
}
iter++;
//cout<<iter<<endl;
if(params.max_iter>0 && iter>=params.max_iter)
{
ret = SOLVER_STATUS_MAX_ITER;
break;
}
}
return ret;
}
IGL_INLINE bool igl::lu_lagrange(
const Eigen::SparseMatrix<T> & ATA,
const Eigen::SparseMatrix<T> & C,
Eigen::SparseMatrix<T> & L,
Eigen::SparseMatrix<T> & U)
{
#if EIGEN_VERSION_AT_LEAST(3,0,92)
#if defined(_WIN32)
#pragma message("lu_lagrange has not yet been implemented for your Eigen Version")
#else
#warning lu_lagrange has not yet been implemented for your Eigen Version
#endif
return false;
#else
// number of unknowns
int n = ATA.rows();
// number of lagrange multipliers
int m = C.cols();
assert(ATA.cols() == n);
if(m != 0)
{
assert(C.rows() == n);
if(C.nonZeros() == 0)
{
// See note above about empty columns in C
fprintf(stderr,"Error: lu_lagrange() C has columns but no entries\n");
return false;
}
}
// Check that each column of C has at least one entry
std::vector<bool> has_entry; has_entry.resize(C.cols(),false);
// Iterate over outside
for(int k=0; k<C.outerSize(); ++k)
{
// Iterate over inside
for(typename Eigen::SparseMatrix<T>::InnerIterator it (C,k); it; ++it)
{
has_entry[it.col()] = true;
}
}
for(int i=0;i<(int)has_entry.size();i++)
{
if(!has_entry[i])
{
// See note above about empty columns in C
fprintf(stderr,"Error: lu_lagrange() C(:,%d) has no entries\n",i);
return false;
}
}
// Cholesky factorization of ATA
//// Eigen fails if you give a full view of the matrix like this:
//Eigen::SparseLLT<SparseMatrix<T> > ATA_LLT(ATA);
Eigen::SparseMatrix<T> ATA_LT = ATA.template triangularView<Eigen::Lower>();
Eigen::SparseLLT<Eigen::SparseMatrix<T> > ATA_LLT(ATA_LT);
Eigen::SparseMatrix<T> J = ATA_LLT.matrixL();
//if(!ATA_LLT.succeeded())
if(!((J*0).eval().nonZeros() == 0))
{
fprintf(stderr,"Error: lu_lagrange() failed to factor ATA\n");
return false;
}
if(m == 0)
{
// If there are no constraints (C is empty) then LU decomposition is just L
// and L' from cholesky decomposition
L = J;
U = J.transpose();
}else
{
// Construct helper matrix M
Eigen::SparseMatrix<T> M = C;
J.template triangularView<Eigen::Lower>().solveInPlace(M);
// Compute cholesky factorizaiton of M'*M
Eigen::SparseMatrix<T> MTM = M.transpose() * M;
Eigen::SparseLLT<Eigen::SparseMatrix<T> > MTM_LLT(MTM.template triangularView<Eigen::Lower>());
Eigen::SparseMatrix<T> K = MTM_LLT.matrixL();
//if(!MTM_LLT.succeeded())
if(!((K*0).eval().nonZeros() == 0))
{
fprintf(stderr,"Error: lu_lagrange() failed to factor MTM\n");
return false;
}
// assemble LU decomposition of Q
Eigen::Matrix<int,Eigen::Dynamic,1> MI;
Eigen::Matrix<int,Eigen::Dynamic,1> MJ;
Eigen::Matrix<T,Eigen::Dynamic,1> MV;
igl::find(M,MI,MJ,MV);
Eigen::Matrix<int,Eigen::Dynamic,1> KI;
Eigen::Matrix<int,Eigen::Dynamic,1> KJ;
Eigen::Matrix<T,Eigen::Dynamic,1> KV;
igl::find(K,KI,KJ,KV);
Eigen::Matrix<int,Eigen::Dynamic,1> JI;
Eigen::Matrix<int,Eigen::Dynamic,1> JJ;
Eigen::Matrix<T,Eigen::Dynamic,1> JV;
igl::find(J,JI,JJ,JV);
int nnz = JV.size() + MV.size() + KV.size();
Eigen::Matrix<int,Eigen::Dynamic,1> UI(nnz);
Eigen::Matrix<int,Eigen::Dynamic,1> UJ(nnz);
Eigen::Matrix<T,Eigen::Dynamic,1> UV(nnz);
UI << JJ, MI, (KJ.array() + n).matrix();
UJ << JI, (MJ.array() + n).matrix(), (KI.array() + n).matrix();
UV << JV, MV, KV*-1;
igl::sparse(UI,UJ,UV,U);
Eigen::Matrix<int,Eigen::Dynamic,1> LI(nnz);
Eigen::Matrix<int,Eigen::Dynamic,1> LJ(nnz);
Eigen::Matrix<T,Eigen::Dynamic,1> LV(nnz);
LI << JI, (MJ.array() + n).matrix(), (KI.array() + n).matrix();
LJ << JJ, MI, (KJ.array() + n).matrix();
LV << JV, MV, KV;
igl::sparse(LI,LJ,LV,L);
}
return true;
#endif
}
Eigen::Matrix<Scalar, Eigen::Dynamic, 1> squareVector(Eigen::Matrix<Scalar, Eigen::Dynamic, 1> x) {
return x.array() * x.array();
}
Eigen::Matrix<Scalar, Eigen::Dynamic, ColsAtCompileTime> squareMatrix(Eigen::Matrix<Scalar, Eigen::Dynamic, ColsAtCompileTime> x) {
return x.array() * x.array();
}