/* Solve min||Ax-b|| for a matrix A whose rank is given. */
VectorXd solve( const VectorXd& b , bool inY=false)
{
if( rank==0 ) return VectorXd::Zero(NC);
/* Approximate solution using no basis transfo (result is meanigless
* appart from the computation time pov. */
/*
VectorXd sol = b.head(rank);
matrixL().solveInPlace( sol );
VectorXd res; res.setZero(NC);
res.head(rank)=sol; return res;
*/
/* With plain matrices. */
/*
VectorXd sol = matrixUr().transpose()*b;
matrixL().solveInPlace( sol );
return matrixVr()*sol;
*/
/* Using the HH representation of V. */
assert( m_computeThinU || m_computeFullU );
VectorXd sol;
if( inY ) sol.setZero(rank); else sol.setZero(NC);
sol.head(rank) = matrixUr().transpose()*b;
matrixL().solveInPlace( sol.head(rank) );
if( ! inY ) sol.applyOnTheLeft(qrv.householderQ());
return sol;
}
// 2 dimensional collision check. Return true if point is contained inside rectangle
bool exists_collision(VectorXd x_near, VectorXd x_new) {
// ONLY WORKS WITH EIGEN3. IF FOR SOME REASON YOU ARE RUNNING THIS SOMEWHERE ELSE,
// HARD CODE THIS
x_near = x_near.head(2);
x_new = x_new.head(2);
MatrixXd obstacles = setup_values.obstacles;
int num_obstacles = obstacles.cols();
for (int n = 0; n < num_obstacles; n++) {
double x_min = obstacles(0,n) - .5*obstacles(1,n);
double x_max = obstacles(0,n) + .5*obstacles(1,n);
double y_min = obstacles(2,n) - .5*obstacles(3,n);
double y_max = obstacles(2,n) + .5*obstacles(3,n);
// Determine if point is inside obstacle by discretization
float dis_num = 20;
VectorXd direction = (x_new - x_near)/(x_new - x_near).norm();
double length = (x_new - x_near).norm();
for (int i = 1;i <= dis_num; i++) {
Vector2d point = x_near + length * i/dis_num * direction;
if (inside_rectangular_obs(point, x_min, x_max, y_min, y_max)) {
return true;
}
}
}
return false;
}
double whimed_i(
VectorXd& a,
VectorXi& w,
int n,
VectorXd& a_cand,
VectorXd& a_psrt,
VectorXi& w_cand
){
//Eigen-ized version of whimed_i. See citation("pcaPP")
int n2,i,k_cand,nn=n;/* sum of weights: `int' do overflow when n ~>= 1e5 */
int64_t wleft,wmid,wright,w_tot,wrest=0;
double trial;
w_tot=w.head(n).sum();
do{
a_psrt.head(nn)=a.head(nn);
n2=nn/2;
std::nth_element(a_psrt.data(),a_psrt.data()+n2,a_psrt.data()+nn);
trial=a_psrt(n2);
wleft=0;wmid=0;wright=0;
for (i=0;i<nn;++i){
if(a(i)<trial) wleft+=w(i);
else if(a(i)>trial) wright+=w(i);
else wmid+=w(i);
}
k_cand=0;
if((2*(wrest+wleft))>w_tot){
for (i=0;i<nn;++i){
if(a(i)<trial){
a_cand(k_cand)=a(i);
w_cand(k_cand)=w(i);
++k_cand;
}
}
}
else if((2*(wrest+wleft+wmid))<=w_tot){
for (i=0;i<nn;++i){
if (a(i)>trial){
a_cand(k_cand)=a(i);
w_cand(k_cand)=w(i);
++k_cand;
}
}
wrest+=wleft+wmid;
} else {
return(trial);
}
nn=k_cand;
a.head(nn)=a_cand.head(nn);
w.head(nn)=w_cand.head(nn);
} while(1);
}
void LCPSolver::transferSolFromODEFormulation(const VectorXd& _x, VectorXd& xOut, int numDir)
{
int numContacts = _x.size() / 3;
xOut = VectorXd::Zero(numContacts * (2 + numDir));
xOut.head(numContacts) = _x.head(numContacts);
int offset = numDir / 4;
for (int i = 0; i < numContacts; ++i)
{
xOut[numContacts + i * numDir + 0] = _x[numContacts + i * 2 + 0];
xOut[numContacts + i * numDir + offset] = _x[numContacts + i * 2 + 1];
}
}
/**
* reconstruct the displacements u in Euler space with RS coordinates y
* provided.
*
* @param y the RS coordinates.
* @param u the constructed Euler coordinates represents the displacements.
*
* @return true if construction is success.
*/
bool RS2Euler::reconstruct(const VectorXd &y, VectorXd &u){
assert (tetmesh != NULL);
const int node_numx3 = tetmesh->nodes().size()*3;
bool succ = true;
// assemble the right_side
VectorXd b;
assemble_b(y,b);
assert_eq(VG_t.cols(),b.size());
assert_eq(VG_t.rows(),node_numx3);
VectorXd right_side(node_numx3 + numFixedDofs());
right_side.segment(0, node_numx3) = VG_t*b;
right_side.segment(node_numx3,numFixedDofs()).setZero();
right_side.segment(node_numx3,barycenter_uc.size()) = barycenter_uc;
// solve A*u = right_side
assert_eq (right_side.size(), A_solver.rows());
u = A_solver.solve(right_side);
// get the result
if(A_solver.info()!=Eigen::Success) {
succ = false;
u.resize(node_numx3);
u.setZero();
ERROR_LOG("failed to solve for A X = P.");
}else{
assert_gt(u.size(), node_numx3);
const VectorXd x = u.head(node_numx3);
u = x;
}
return succ;
}
void MSCKF::addSlideState()
{
SlideState newState;
int nominalStateLength = (int)fullNominalState.size();
int errorStateLength = (int)fullErrorCovariance.rows();
VectorXd tmpNominal = VectorXd::Zero(nominalStateLength + NOMINAL_POSE_STATE_SIZE);
MatrixXd tmpCovariance = MatrixXd::Zero(errorStateLength + ERROR_POSE_STATE_SIZE, errorStateLength + ERROR_POSE_STATE_SIZE);
MatrixXd Jpi = MatrixXd::Zero(9, errorStateLength);
tmpNominal.head(nominalStateLength) = fullNominalState;
tmpNominal.segment(nominalStateLength, NOMINAL_POSE_STATE_SIZE) = fullNominalState.head(NOMINAL_POSE_STATE_SIZE);
fullNominalState = tmpNominal;
newState.q = fullNominalState.head(4);
newState.p = fullNominalState.segment(4, 3);
newState.v = fullNominalState.segment(7, 3);
slidingWindow.push_back(newState);
Jpi.block<3,3>(0, 0) = Matrix3d::Identity(3, 3);
Jpi.block<3,3>(3, 3) = Matrix3d::Identity(3, 3);
Jpi.block<3,3>(6, 6) = Matrix3d::Identity(3, 3);
tmpCovariance.block(0, 0, errorStateLength, errorStateLength) = fullErrorCovariance;
tmpCovariance.block(errorStateLength, 0, 9, errorStateLength) = Jpi * fullErrorCovariance;
tmpCovariance.block(0, errorStateLength, errorStateLength, 9) = fullErrorCovariance * Jpi.transpose();
tmpCovariance.block<ERROR_POSE_STATE_SIZE, ERROR_POSE_STATE_SIZE>(errorStateLength, errorStateLength) =
Jpi * fullErrorCovariance * Jpi.transpose();
fullErrorCovariance = tmpCovariance;
}
VectorXd HarmonicGradient(const vector<spring> &springlist,
const VectorXd &XY,
const double g11,
const double g12,
const double g22)
{
VectorXd gradE(XY.size());
for(int i=0;i<gradE.size();i++){
gradE(i)=0;
}
VectorXd X(XY.size()/2);
VectorXd Y(XY.size()/2);
X=XY.head(XY.size()/2);
Y=XY.tail(XY.size()/2);
for(int i=0;i<springlist.size();i++){
int one=springlist[i].one;
int two=springlist[i].two;
int num=XY.size()/2;
double dx=X(one)-(X(two)+springlist[i].wlr);
double dy=Y(one)-(Y(two)+springlist[i].wud);
double k=springlist[i].k;
double L=springlist[i].rlen;
double dist=sqrt( g11*dx*dx+ 2*g12*dx*dy+ g22*dy*dy );
double gradx= k*(dist-L)*(g11*dx+g12*dy)/dist;
double grady= k*(dist-L)*(g22*dy+g12*dx)/dist;
gradE(one) += gradx;
gradE(two) -= gradx;
gradE(one+num) += grady;
gradE(two+num) -= grady;
}
return gradE;
}
double Model::score(const VectorXd& psi) const
{
ROS_ASSERT(psi.rows() == eweights_.rows() + nweights_.rows());
double val = psi.head(eweights_.rows()).dot(eweights_);
val += psi.tail(nweights_.rows()).dot(nweights_);
return val;
}
void Optimizer::augment_sparse_linear_system(SparseSystem& W,
const Properties& prop) {
if (prop.method == DOG_LEG) {
// We're using the incremental version of Powell's Dog-Leg, so we need
// to form the updated gradient.
const VectorXd& f_new = W.rhs();
// Augment the running count for the sum-of-squared errors at the current
// linearization point.
current_SSE_at_linpoint += f_new.squaredNorm();
// Allocate the new gradient vector
VectorXd g_new(W.num_cols());
// Compute W^T \cdot f_new
VectorXd increment = mul_SparseMatrixTrans_Vector(W, f_new);
// Set g_new = (g_old 0)^T + W^T f_new.
g_new.head(gradient.size()) = gradient + increment.head(gradient.size());
g_new.tail(W.num_cols() - gradient.size()) = increment.tail(
W.num_cols() - gradient.size());
// Cache the new gradient vector
gradient = g_new;
}
// Apply Givens to QR factorize the newly augmented sparse system.
for (int i = 0; i < W.num_rows(); i++) {
SparseVector new_row = W.get_row(i);
function_system._R.add_row_givens(new_row, W.rhs()(i));
}
}
int unimcd(
VectorXd& y,
const int& n,
const int& h,
const int& len,
double& initmean,
double& initcov
){
//does NOT accept ties in the y's!
std::sort(y.data(),y.data()+y.size());
VectorXd ay(len);
ay(0)=y.head(h).sum();
for(int samp=1;samp<len;samp++) ay(samp)=ay(samp-1)-y(samp-1)+y(samp+h-1);
VectorXd ay2(len);
ay2=ay.array().square()/(double)h;
VectorXd y2(n);
y2=y.array().square();
VectorXd sq(len);
sq(0)=y2.head(h).sum()-ay2(0);
for(int samp=1;samp<len;samp++) sq(samp)=sq(samp-1)-y2(samp-1)+y2(samp+h-1)-ay2(samp)+ay2(samp-1);
int minone;
initcov=sq.minCoeff(&minone);
initcov/=(double)(h-1);
initmean=ay(minone)/(double)h;
return(minone);
}
// Implements Equation S12 in the Supplementary
MatrixXd expected_dissimilarities(const MatrixXd &J, const MatrixXd &M, const VectorXd &W) {
int n = J.rows();
int o = J.cols();
VectorXd JW = J*W.head(o);
VectorXd u_n = VectorXd::Ones(n);
MatrixXd Delta = J*resistance_distance(M,o)*J.transpose();
Delta.noalias() += 0.5*JW*u_n.transpose();
Delta.noalias() += 0.5*u_n*JW.transpose();
Delta.diagonal() -= JW;
return (Delta);
}
bool inBounds(VectorXd state) {
Vector2d pos, vel;
pos = state.head(2);
vel = state.tail(2);
if (inside_rectangular_obs(pos, setup_values.x_min, setup_values.x_max,
setup_values.x_min, setup_values.x_max) &&
inside_rectangular_obs(vel, setup_values.v_min, setup_values.v_max,
setup_values.v_min, setup_values.v_max)) {
return true;
}
return false;
}
VectorXd CurvatureError::operator()(const VectorXd& a) const {
DblVec curvatures;
DblVec Deltas;
curvatures = toDblVec(a.head(pi->curvature_vars.size()));
Deltas = DblVec(pi->curvature_vars.size(), a(a.size() - 1));
VectorXd ret(1);
ret(0) = -this->total_curvature_limit;
for (int i = 0; i < curvatures.size(); ++i) {
ret(0) += fabs(curvatures[i]) * Deltas[i];
}
return ret;
}
IGL_INLINE bool igl::linprog(
const Eigen::VectorXd & f,
const Eigen::MatrixXd & A,
const Eigen::VectorXd & b,
const Eigen::MatrixXd & B,
const Eigen::VectorXd & c,
Eigen::VectorXd & x)
{
using namespace Eigen;
using namespace std;
const int m = A.rows();
const int n = A.cols();
const int p = B.rows();
MatrixXd Im = MatrixXd::Identity(m,m);
MatrixXd AS(m,n+m);
AS<<A,Im;
MatrixXd bS = b.array().abs();
for(int i = 0;i<m;i++)
{
const auto & sign = [](double x)->double
{
return (x<0?-1:(x>0?1:0));
};
AS.row(i) *= sign(b(i));
}
MatrixXd In = MatrixXd::Identity(n,n);
MatrixXd P(n+m,2*n+m);
P<< In, -In, MatrixXd::Zero(n,m),
MatrixXd::Zero(m,2*n), Im;
MatrixXd ASP = AS*P;
MatrixXd BSP(0,2*n+m);
if(p>0)
{
MatrixXd BS(p,2*n);
BS<<B,MatrixXd::Zero(p,n);
BSP = BS*P;
}
VectorXd fSP = VectorXd::Ones(2*n+m);
fSP.head(2*n) = P.block(0,0,n,2*n).transpose()*f;
const VectorXd & cc = fSP;
MatrixXd AA(m+p,2*n+m);
AA<<ASP,BSP;
VectorXd bb(m+p);
bb<<bS,c;
VectorXd xxs;
bool ret = linprog(cc,AA,bb,0,xxs);
x = P.block(0,0,n,2*n+m)*xxs;
return ret;
}
double CurvatureCost::operator()(const VectorXd& a) const {
DblVec curvatures;
DblVec Deltas;
curvatures = toDblVec(a.head(pi->curvature_vars.size()));
Deltas = DblVec(pi->curvature_vars.size(), a(a.size() - 1));
double ret = 0;
for (int i = 0; i < curvatures.size(); ++i)
{
double tmp = curvatures[i] * Deltas[i];
ret += tmp * tmp;
}
return ret * coeff;
}
void MotionModel::predict_state_and_covariance(VectorXd x_k_k, MatrixXd p_k_k, string type, double std_a,
double std_alpha, VectorXd *X_km1_k, MatrixXd *P_km1_k)
{
size_t x_k_size = x_k_k.size(), p_k_size = p_k_k.rows();
double delta_t = 1, linear_acceleration_noise_covariance, angular_acceleration_noise_covariance;
VectorXd Xv_km1_k(18), Pn_diag(6);
MatrixXd F = MatrixXd::Identity(18,18), // The camera state size is assumed to be 18
Pn, Q, G;
// Camera motion prediction
fv(x_k_k.head(18), delta_t, type, std_a, std_alpha, &Xv_km1_k);
// Feature prediction
(*X_km1_k).resize(x_k_size);
// Add the feature points to the state vector after cam motion prediction
(*X_km1_k) << Xv_km1_k, x_k_k.tail(x_k_size - 18);
// State transition equation derivatives
dfv_by_dxv(x_k_k.head(18), delta_t, type, &F);
// State noise
linear_acceleration_noise_covariance = (std_a * delta_t) * (std_a * delta_t);
angular_acceleration_noise_covariance = (std_alpha * delta_t) * (std_alpha * delta_t);
Pn_diag << linear_acceleration_noise_covariance, linear_acceleration_noise_covariance, linear_acceleration_noise_covariance,
angular_acceleration_noise_covariance, angular_acceleration_noise_covariance, angular_acceleration_noise_covariance;
Pn = Pn_diag.asDiagonal();
func_Q(x_k_k.head(18), Pn, delta_t, type, &G, &Q);
// P_km1_k resize and update
// With the property of symmetry for the covariance matrix, only the Pxx and Pxy
// which means the camera state covariance and camera feature points covariance can be calculated
(*P_km1_k).resize(p_k_size,p_k_size);
(*P_km1_k).block(0,0,18,18) = F * p_k_k.block(0,0,18,18) * F.transpose() + Q;
(*P_km1_k).block(0,18,18,p_k_size - 18) = F * p_k_k.block(0,18,18,p_k_size - 18);
(*P_km1_k).block(18,0,p_k_size - 18,18) = p_k_k.block(18,0,p_k_size - 18,18) * F.transpose();
(*P_km1_k).block(18,18,p_k_size - 18,p_k_size - 18) = p_k_k.block(18,18,p_k_size - 18,p_k_size - 18);
}
/* Implementation of byfree */
void byfree(VectorXd target_r, VectorXd target_i, VectorXd zetaf_r, VectorXd zetaf_i, VectorXd gammaf, VectorXd& pot_r, VectorXd& pot_i){
/*
timeval time1, time2;
double ttl1, ttl2, diffl12;
gettimeofday(&time1, NULL); ttl1 = (time1.tv_sec * 1000000 + time1.tv_usec);
*/
int nt = target_r.size(), ns = zetaf_i.size();
MatrixXd dx(nt,ns), dy(nt,ns), dz(nt,ns), F_r(nt,ns-1), F_i(nt,ns-1);
VectorXd distr, distr_r(ns-1), distr_i(ns-1);
distr = -(gammaf.tail(ns-1) - gammaf.head(ns-1));
for (int k=0; k<ns-1; k++) {
double dxnorm = (zetaf_r(k+1) - zetaf_r(k))*(zetaf_r(k+1) - zetaf_r(k)) + (zetaf_i(k+1) - zetaf_i(k))*(zetaf_i(k+1) - zetaf_i(k));
distr_r(k) = (zetaf_r(k+1) - zetaf_r(k))*distr(k)/dxnorm;
distr_i(k) = -(zetaf_i(k+1) - zetaf_i(k))*distr(k)/dxnorm;
}
for (int j=0; j<nt; j++) {
for (int k=0; k<ns; k++) {
dx(j,k)=(target_r(j)-zetaf_r(k));
dy(j,k)=(target_i(j)-zetaf_i(k));
dz(j,k)=dx(j,k)*dx(j,k)+dy(j,k)*dy(j,k);
}
}
for (int j=0; j<nt; j++) {
for (int k=0; k<ns-1; k++) {
F_r(j,k) = log(dz(j,k)/dz(j,k+1))/2;
F_i(j,k) = atan2((-dx(j,k)*dy(j,k+1)+dy(j,k)*dx(j,k+1)),(dx(j,k)*dx(j,k+1)+dy(j,k)*dy(j,k+1)));
}
}
pot_r = F_r*distr_r - F_i*distr_i;
pot_i = F_i*distr_r + F_r*distr_i;;
/*
gettimeofday(&time2, NULL); ttl2 = (time2.tv_sec * 1000000 + time2.tv_usec);
diffl12 = (double)(ttl2 - ttl1)/1000000;
printf("Elapsed %f seconds!\n", diffl12);
*/
return ;
}
MatrixXd compute_w( SparseMatrix<double> M2, int KHID ){
cout << "Computing the whitening matrix..." << endl;
int m = M2.rows();
cout << "Calculating SVD..." << endl;
JacobiSVD<MatrixXd> svd( M2, ComputeThinU );
cout << "SVD successfully calculated" << endl;
VectorXd E = svd.singularValues();
E = E.head(KHID);
E = E.array().sqrt().inverse().matrix();
MatrixXd E_diag = E.asDiagonal();
MatrixXd U = svd.matrixU();
U = U.block(0,0,m,KHID);
cout << U.rows() << endl;
cout << U.cols() << endl;
return U*E_diag;
}
void QPSolver::postSolve(const VectorXd& sol, bool status, VectorXd& realSol)
{
if (status)
{
if (mMethodType == SM_Normal)
{
realSol = sol;
}
else if (mMethodType == SM_SVD)
{
realSol = mU * sol;
}
else if (mMethodType == SM_Separable)
{
realSol = sol.head(mNumVarOriginal);
}
}
}
VectorXd MyWindow::evalDeriv() {
// The root follows the desired acceleration and the rest of the body follows dynamic equations. The root acceleration will impact the rest of the body correctly. Collision or other external forces will alter the root acceleration
setPose();
VectorXd deriv = VectorXd::Zero(mIndices.back() * 2);
for (unsigned int i = 0; i < mSkels.size(); i++) {
// skip immobile objects in forward simulation
if (mSkels[i]->getImmobileState())
continue;
int start = mIndices[i] * 2;
int size = mDofs[i].size();
Vector3d desiredAccel(0.0, 0.0, 0.0);
if (mSimFrame > 500 && mSimFrame < 800)
desiredAccel[0] = 5.0;
else if (mSimFrame > 800 && mSimFrame < 1100)
desiredAccel[0] = -7.0;
else if (mSimFrame > 1100 && mSimFrame < 1400)
desiredAccel[0] = 6.0;
else if (mSimFrame > 1400 && mSimFrame < 1700)
desiredAccel[0] = -5.0;
else if (mSimFrame > 1700 && mSimFrame < 2000)
desiredAccel[0] = 5.0;
else if (mSimFrame > 2000 && mSimFrame < 2300)
desiredAccel[0] = -5.0;
int nDof = mSkels[i]->getNumDofs();
MatrixXd J(3, nDof);
J.setZero();
J(0, 0) = 1.0;
J(1, 1) = 1.0;
J(2, 2) = 1.0;
MatrixXd A = mSkels[i]->getMassMatrix();
A.block(0, 0, nDof, 3) = J.transpose();
VectorXd b = -mSkels[i]->getCombinedVector() + mSkels[i]->getInternalForces() - mSkels[i]->getMassMatrix().block(0, 0, nDof, 3) * desiredAccel;
VectorXd x = A.inverse() * b;
VectorXd qddot = x;
qddot.head(3) = desiredAccel;
mSkels[i]->clampRotation(mDofs[i], mDofVels[i]);
deriv.segment(start, size) = mDofVels[i] + (qddot * mTimeStep); // set velocities
deriv.segment(start + size, size) = qddot; // set qddot (accelerations)
}
return deriv;
}
void MotionModel::fv(VectorXd x_k_k, double delta_t, string type, double std_a, double std_alpha, VectorXd *Xv_km1_k)
{
// This function updates the state for the camera. See Javier Civera book P39. 3.2.1 or P129 A.9
// It assumes the camera has a constant linear and angular velocity, so the
// linear and angular accelerations are zero.
VectorXd calibration, rW, qWR, vW, wW, Q, QP; // The camera state size is assumed to be 18
calibration = x_k_k.head(5); // Five intrinsic parameters are assumed
rW = x_k_k.segment(5,3); // Camera optical center position
qWR = x_k_k.segment(8,4); // Quaternion defines orientation
vW = x_k_k.segment(12,3); // Linear velocity
wW = x_k_k.segment(15,3); // Angular velocity
if (type.compare("constant_velocity") == 0)
{
Q = V2Q(wW * delta_t); // Converts rotation vector to quaternion
QP = Qprod(qWR, Q); // Cross product of qWR and q((wW+OmegaW)*delta_t), where OmegaW is set to 0
*Xv_km1_k << calibration, rW + vW * delta_t, QP, vW, wW; // State update for the camera
}
// The types below are probably not used
else if (type.compare("constant_orientation") == 0)
{
}
else if (type.compare("constant_position") == 0)
{
}
else if (type.compare("constant_position_and_orientation") == 0)
{
}
else if (type.compare("constant_position_and_orientation_location_noise") == 0)
{
}
}
void GPWithRankPrior::copySettings(const GPWithRankPrior & model)
{
//TODO Need Update One Data Changed
// Make sure number of data points is equal.
assert(model.mX.rows() == mX.rows());
// Copy parameters that don't require special processing.
this->mSequence = model.mSequence;
this->mDataMatrix = model.mDataMatrix;
this->mY = model.mY;
// Copy latent positions or convert them to desired dimensionality.
if (model.mX.cols() == mX.cols())
{
mX = model.mX;
}
else
{
// Pull out the largest singular values to keep.
JacobiSVD<MatrixXd> svd(model.mX, ComputeThinU | ComputeThinV);
VectorXd S = svd.singularValues();
this->mX = svd.matrixU().block(0,0,this->mX.rows(),this->mX.cols())*S.head(this->mX.cols()).asDiagonal();
std::cout << mX << std::endl;
// Report on the singular values that are kept and discarded.
DBPRINTLN("Largest singular value discarded: " << S(this->mX.cols()));
DBPRINTLN("Smallest singular value kept: " << S(this->mX.cols()-1));
DBPRINTLN("Average singular value kept: " << ((1.0/((double)this->mX.cols()))*S.head(this->mX.cols()).sum()));
DBPRINT("Singular values: ");
DBPRINTMAT(S);
}
if (mReconstructionGP) mReconstructionGP->copySettings(model.mReconstructionGP);
if (mBackConstraint) mBackConstraint->copySettings(model.mBackConstraint);
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
int error;
if (nrhs<1) mexErrMsgTxt("usage: ptr = QPControllermex(0,control_obj,robot_obj,...); alpha=QPControllermex(ptr,...,...)");
if (nlhs<1) mexErrMsgTxt("take at least one output... please.");
struct QPControllerData* pdata;
mxArray* pm;
double* pr;
int i,j;
if (mxGetScalar(prhs[0])==0) { // then construct the data object and return
pdata = new struct QPControllerData;
// get control object properties
const mxArray* pobj = prhs[1];
pm = myGetProperty(pobj,"slack_limit");
pdata->slack_limit = mxGetScalar(pm);
pm = myGetProperty(pobj,"W_kdot");
assert(mxGetM(pm)==3); assert(mxGetN(pm)==3);
pdata->W_kdot.resize(mxGetM(pm),mxGetN(pm));
memcpy(pdata->W_kdot.data(),mxGetPr(pm),sizeof(double)*mxGetM(pm)*mxGetN(pm));
pm= myGetProperty(pobj,"w_grf");
pdata->w_grf = mxGetScalar(pm);
pm= myGetProperty(pobj,"w_slack");
pdata->w_slack = mxGetScalar(pm);
pm = myGetProperty(pobj,"Kp_ang");
pdata->Kp_ang = mxGetScalar(pm);
pm = myGetProperty(pobj,"Kp_accel");
pdata->Kp_accel = mxGetScalar(pm);
pm= myGetProperty(pobj,"n_body_accel_inputs");
pdata->n_body_accel_inputs = (int) mxGetScalar(pm);
pm= myGetProperty(pobj,"n_body_accel_bounds");
pdata->n_body_accel_bounds = (int) mxGetScalar(pm);
mxArray* body_accel_bounds = myGetProperty(pobj,"body_accel_bounds");
Vector6d vecbound;
for (int i=0; i<pdata->n_body_accel_bounds; i++) {
pdata->accel_bound_body_idx.push_back((int) mxGetScalar(mxGetField(body_accel_bounds,i,"body_idx"))-1);
pm = mxGetField(body_accel_bounds,i,"min_acceleration");
assert(mxGetM(pm)==6); assert(mxGetN(pm)==1);
memcpy(vecbound.data(),mxGetPr(pm),sizeof(double)*6);
pdata->min_body_acceleration.push_back(vecbound);
pm = mxGetField(body_accel_bounds,i,"max_acceleration");
assert(mxGetM(pm)==6); assert(mxGetN(pm)==1);
memcpy(vecbound.data(),mxGetPr(pm),sizeof(double)*6);
pdata->max_body_acceleration.push_back(vecbound);
}
pm = myGetProperty(pobj,"body_accel_input_weights");
pdata->body_accel_input_weights.resize(pdata->n_body_accel_inputs);
memcpy(pdata->body_accel_input_weights.data(),mxGetPr(pm),sizeof(double)*pdata->n_body_accel_inputs);
pdata->n_body_accel_eq_constraints = 0;
for (int i=0; i<pdata->n_body_accel_inputs; i++) {
if (pdata->body_accel_input_weights(i) < 0)
pdata->n_body_accel_eq_constraints++;
}
// get robot mex model ptr
if (!mxIsNumeric(prhs[2]) || mxGetNumberOfElements(prhs[2])!=1)
mexErrMsgIdAndTxt("Drake:QPControllermex:BadInputs","the third argument should be the robot mex ptr");
memcpy(&(pdata->r),mxGetData(prhs[2]),sizeof(pdata->r));
pdata->B.resize(mxGetM(prhs[3]),mxGetN(prhs[3]));
memcpy(pdata->B.data(),mxGetPr(prhs[3]),sizeof(double)*mxGetM(prhs[3])*mxGetN(prhs[3]));
int nq = pdata->r->num_dof, nu = pdata->B.cols();
pm = myGetProperty(pobj,"w_qdd");
pdata->w_qdd.resize(nq);
memcpy(pdata->w_qdd.data(),mxGetPr(pm),sizeof(double)*nq);
pdata->umin.resize(nu);
pdata->umax.resize(nu);
memcpy(pdata->umin.data(),mxGetPr(prhs[4]),sizeof(double)*nu);
memcpy(pdata->umax.data(),mxGetPr(prhs[5]),sizeof(double)*nu);
pdata->B_act.resize(nu,nu);
pdata->B_act = pdata->B.bottomRows(nu);
// get the map ptr back from matlab
if (!mxIsNumeric(prhs[6]) || mxGetNumberOfElements(prhs[6])!=1)
mexErrMsgIdAndTxt("Drake:QPControllermex:BadInputs","the seventh argument should be the map ptr");
memcpy(&pdata->map_ptr,mxGetPr(prhs[6]),sizeof(pdata->map_ptr));
// pdata->map_ptr = NULL;
if (!pdata->map_ptr)
mexWarnMsgTxt("Map ptr is NULL. Assuming flat terrain at z=0");
// create gurobi environment
error = GRBloadenv(&(pdata->env),NULL);
// set solver params (http://www.gurobi.com/documentation/5.5/reference-manual/node798#sec:Parameters)
mxArray* psolveropts = myGetProperty(pobj,"gurobi_options");
int method = (int) mxGetScalar(myGetField(psolveropts,"method"));
CGE ( GRBsetintparam(pdata->env,"outputflag",0), pdata->env );
CGE ( GRBsetintparam(pdata->env,"method",method), pdata->env );
// CGE ( GRBsetintparam(pdata->env,"method",method), pdata->env );
CGE ( GRBsetintparam(pdata->env,"presolve",0), pdata->env );
if (method==2) {
CGE ( GRBsetintparam(pdata->env,"bariterlimit",20), pdata->env );
CGE ( GRBsetintparam(pdata->env,"barhomogeneous",0), pdata->env );
CGE ( GRBsetdblparam(pdata->env,"barconvtol",0.0005), pdata->env );
}
mxClassID cid;
if (sizeof(pdata)==4) cid = mxUINT32_CLASS;
else if (sizeof(pdata)==8) cid = mxUINT64_CLASS;
else mexErrMsgIdAndTxt("Drake:constructModelmex:PointerSize","Are you on a 32-bit machine or 64-bit machine??");
plhs[0] = mxCreateNumericMatrix(1,1,cid,mxREAL);
memcpy(mxGetData(plhs[0]),&pdata,sizeof(pdata));
// preallocate some memory
pdata->H.resize(nq,nq);
pdata->H_float.resize(6,nq);
pdata->H_act.resize(nu,nq);
pdata->C.resize(nq);
pdata->C_float.resize(6);
pdata->C_act.resize(nu);
pdata->J.resize(3,nq);
pdata->Jdot.resize(3,nq);
pdata->J_xy.resize(2,nq);
pdata->Jdot_xy.resize(2,nq);
pdata->Hqp.resize(nq,nq);
pdata->fqp.resize(nq);
pdata->Ag.resize(6,nq);
pdata->Agdot.resize(6,nq);
pdata->Ak.resize(3,nq);
pdata->Akdot.resize(3,nq);
pdata->vbasis_len = 0;
pdata->cbasis_len = 0;
pdata->vbasis = NULL;
pdata->cbasis = NULL;
return;
}
// first get the ptr back from matlab
if (!mxIsNumeric(prhs[0]) || mxGetNumberOfElements(prhs[0])!=1)
mexErrMsgIdAndTxt("Drake:QPControllermex:BadInputs","the first argument should be the ptr");
memcpy(&pdata,mxGetData(prhs[0]),sizeof(pdata));
// for (i=0; i<pdata->r->num_bodies; i++)
// mexPrintf("body %d (%s) has %d contact points\n", i, pdata->r->bodies[i].linkname.c_str(), pdata->r->bodies[i].contact_pts.cols());
int nu = pdata->B.cols(), nq = pdata->r->num_dof;
const int dim = 3, // 3D
nd = 2*m_surface_tangents; // for friction cone approx, hard coded for now
assert(nu+6 == nq);
int narg=1;
int use_fast_qp = (int) mxGetScalar(prhs[narg++]);
Map< VectorXd > qddot_des(mxGetPr(prhs[narg++]),nq);
double *q = mxGetPr(prhs[narg++]);
double *qd = &q[nq];
vector<VectorXd,aligned_allocator<VectorXd>> body_accel_inputs;
for (int i=0; i<pdata->n_body_accel_inputs; i++) {
assert(mxGetM(prhs[narg])==7); assert(mxGetN(prhs[narg])==1);
VectorXd v = VectorXd::Zero(7,1);
memcpy(v.data(),mxGetPr(prhs[narg++]),sizeof(double)*7);
body_accel_inputs.push_back(v);
}
int num_condof;
VectorXd condof;
if (!mxIsEmpty(prhs[narg])) {
assert(mxGetN(prhs[narg])==1);
num_condof=mxGetM(prhs[narg]);
condof = VectorXd::Zero(num_condof);
memcpy(condof.data(),mxGetPr(prhs[narg++]),sizeof(double)*num_condof);
}
else {
num_condof=0;
narg++; // skip over empty vector
}
int desired_support_argid = narg++;
Map<MatrixXd> A_ls(mxGetPr(prhs[narg]),mxGetM(prhs[narg]),mxGetN(prhs[narg])); narg++;
Map<MatrixXd> B_ls(mxGetPr(prhs[narg]),mxGetM(prhs[narg]),mxGetN(prhs[narg])); narg++;
Map<MatrixXd> Qy (mxGetPr(prhs[narg]),mxGetM(prhs[narg]),mxGetN(prhs[narg])); narg++;
Map<MatrixXd> R_ls(mxGetPr(prhs[narg]),mxGetM(prhs[narg]),mxGetN(prhs[narg])); narg++;
Map<MatrixXd> C_ls(mxGetPr(prhs[narg]),mxGetM(prhs[narg]),mxGetN(prhs[narg])); narg++;
Map<MatrixXd> D_ls(mxGetPr(prhs[narg]),mxGetM(prhs[narg]),mxGetN(prhs[narg])); narg++;
Map<MatrixXd> S (mxGetPr(prhs[narg]),mxGetM(prhs[narg]),mxGetN(prhs[narg])); narg++;
Map<VectorXd> s1(mxGetPr(prhs[narg]),mxGetM(prhs[narg])); narg++;
Map<VectorXd> s1dot(mxGetPr(prhs[narg]),mxGetM(prhs[narg])); narg++;
double s2dot = mxGetScalar(prhs[narg++]);
Map<VectorXd> x0(mxGetPr(prhs[narg]),mxGetM(prhs[narg])); narg++;
Map<VectorXd> u0(mxGetPr(prhs[narg]),mxGetM(prhs[narg])); narg++;
Map<VectorXd> y0(mxGetPr(prhs[narg]),mxGetM(prhs[narg])); narg++;
Map<VectorXd> qdd_lb(mxGetPr(prhs[narg]),mxGetM(prhs[narg])); narg++;
Map<VectorXd> qdd_ub(mxGetPr(prhs[narg]),mxGetM(prhs[narg])); narg++;
memcpy(pdata->w_qdd.data(),mxGetPr(prhs[narg++]),sizeof(double)*nq);
double mu = mxGetScalar(prhs[narg++]);
double terrain_height = mxGetScalar(prhs[narg++]); // nonzero if we're using DRCFlatTerrainMap
MatrixXd R_DQyD_ls = R_ls + D_ls.transpose()*Qy*D_ls;
pdata->r->doKinematics(q,false,qd);
//---------------------------------------------------------------------
// Compute active support from desired supports -----------------------
vector<SupportStateElement> active_supports;
set<int> contact_bodies; // redundant, clean up later
int num_active_contact_pts=0;
if (!mxIsEmpty(prhs[desired_support_argid])) {
VectorXd phi;
mxArray* mxBodies = myGetField(prhs[desired_support_argid],"bodies");
if (!mxBodies) mexErrMsgTxt("couldn't get bodies");
double* pBodies = mxGetPr(mxBodies);
mxArray* mxContactPts = myGetField(prhs[desired_support_argid],"contact_pts");
if (!mxContactPts) mexErrMsgTxt("couldn't get contact points");
mxArray* mxContactSurfaces = myGetField(prhs[desired_support_argid],"contact_surfaces");
if (!mxContactSurfaces) mexErrMsgTxt("couldn't get contact surfaces");
double* pContactSurfaces = mxGetPr(mxContactSurfaces);
for (i=0; i<mxGetNumberOfElements(mxBodies);i++) {
mxArray* mxBodyContactPts = mxGetCell(mxContactPts,i);
int nc = mxGetNumberOfElements(mxBodyContactPts);
if (nc<1) continue;
SupportStateElement se;
se.body_idx = (int) pBodies[i]-1;
pr = mxGetPr(mxBodyContactPts);
for (j=0; j<nc; j++) {
se.contact_pt_inds.insert((int)pr[j]-1);
}
se.contact_surface = (int) pContactSurfaces[i]-1;
active_supports.push_back(se);
num_active_contact_pts += nc;
contact_bodies.insert((int)se.body_idx);
}
}
pdata->r->HandC(q,qd,(MatrixXd*)NULL,pdata->H,pdata->C,(MatrixXd*)NULL,(MatrixXd*)NULL,(MatrixXd*)NULL);
pdata->H_float = pdata->H.topRows(6);
pdata->H_act = pdata->H.bottomRows(nu);
pdata->C_float = pdata->C.head(6);
pdata->C_act = pdata->C.tail(nu);
bool include_angular_momentum = (pdata->W_kdot.array().maxCoeff() > 1e-10);
if (include_angular_momentum) {
pdata->r->getCMM(q,qd,pdata->Ag,pdata->Agdot);
pdata->Ak = pdata->Ag.topRows(3);
pdata->Akdot = pdata->Agdot.topRows(3);
}
Vector3d xcom;
// consider making all J's into row-major
pdata->r->getCOM(xcom);
pdata->r->getCOMJac(pdata->J);
pdata->r->getCOMJacDot(pdata->Jdot);
pdata->J_xy = pdata->J.topRows(2);
pdata->Jdot_xy = pdata->Jdot.topRows(2);
MatrixXd Jcom,Jcomdot;
if (x0.size()==6) {
Jcom = pdata->J;
Jcomdot = pdata->Jdot;
}
else {
Jcom = pdata->J_xy;
Jcomdot = pdata->Jdot_xy;
}
Map<VectorXd> qdvec(qd,nq);
MatrixXd B,JB,Jp,Jpdot,normals;
int nc = contactConstraintsBV(pdata->r,num_active_contact_pts,mu,active_supports,pdata->map_ptr,B,JB,Jp,Jpdot,normals,terrain_height);
int neps = nc*dim;
VectorXd x_bar,xlimp;
MatrixXd D_float(6,JB.cols()), D_act(nu,JB.cols());
if (nc>0) {
if (x0.size()==6) {
// x,y,z com
xlimp.resize(6);
xlimp.topRows(3) = xcom;
xlimp.bottomRows(3) = Jcom*qdvec;
}
else {
xlimp.resize(4);
xlimp.topRows(2) = xcom.topRows(2);
xlimp.bottomRows(2) = Jcom*qdvec;
}
x_bar = xlimp-x0;
D_float = JB.topRows(6);
D_act = JB.bottomRows(nu);
}
int nf = nc*nd; // number of contact force variables
int nparams = nq+nf+neps;
Vector3d kdot_des;
if (include_angular_momentum) {
VectorXd k = pdata->Ak*qdvec;
kdot_des = -pdata->Kp_ang*k; // TODO: parameterize
}
//----------------------------------------------------------------------
// QP cost function ----------------------------------------------------
//
// min: ybar*Qy*ybar + ubar*R*ubar + (2*S*xbar + s1)*(A*x + B*u) +
// w_qdd*quad(qddot_ref - qdd) + w_eps*quad(epsilon) +
// w_grf*quad(beta) + quad(kdot_des - (A*qdd + Adot*qd))
VectorXd f(nparams);
{
if (nc > 0) {
// NOTE: moved Hqp calcs below, because I compute the inverse directly for FastQP (and sparse Hqp for gurobi)
VectorXd tmp = C_ls*xlimp;
VectorXd tmp1 = Jcomdot*qdvec;
MatrixXd tmp2 = R_DQyD_ls*Jcom;
pdata->fqp = tmp.transpose()*Qy*D_ls*Jcom;
pdata->fqp += tmp1.transpose()*tmp2;
pdata->fqp += (S*x_bar + 0.5*s1).transpose()*B_ls*Jcom;
pdata->fqp -= u0.transpose()*tmp2;
pdata->fqp -= y0.transpose()*Qy*D_ls*Jcom;
pdata->fqp -= (pdata->w_qdd.array()*qddot_des.array()).matrix().transpose();
if (include_angular_momentum) {
pdata->fqp += qdvec.transpose()*pdata->Akdot.transpose()*pdata->W_kdot*pdata->Ak;
pdata->fqp -= kdot_des.transpose()*pdata->W_kdot*pdata->Ak;
}
f.head(nq) = pdata->fqp.transpose();
} else {
f.head(nq) = -qddot_des;
}
}
f.tail(nf+neps) = VectorXd::Zero(nf+neps);
int neq = 6+neps+6*pdata->n_body_accel_eq_constraints+num_condof;
MatrixXd Aeq = MatrixXd::Zero(neq,nparams);
VectorXd beq = VectorXd::Zero(neq);
// constrained floating base dynamics
// H_float*qdd - J_float'*lambda - Dbar_float*beta = -C_float
Aeq.topLeftCorner(6,nq) = pdata->H_float;
beq.topRows(6) = -pdata->C_float;
if (nc>0) {
Aeq.block(0,nq,6,nc*nd) = -D_float;
}
if (nc > 0) {
// relative acceleration constraint
Aeq.block(6,0,neps,nq) = Jp;
Aeq.block(6,nq,neps,nf) = MatrixXd::Zero(neps,nf); // note: obvious sparsity here
Aeq.block(6,nq+nf,neps,neps) = MatrixXd::Identity(neps,neps); // note: obvious sparsity here
beq.segment(6,neps) = (-Jpdot -pdata->Kp_accel*Jp)*qdvec;
}
// add in body spatial equality constraints
VectorXd body_vdot;
MatrixXd orig = MatrixXd::Zero(4,1);
orig(3,0) = 1;
int body_idx;
int equality_ind = 6+neps;
MatrixXd Jb(6,nq);
MatrixXd Jbdot(6,nq);
for (int i=0; i<pdata->n_body_accel_inputs; i++) {
if (pdata->body_accel_input_weights(i) < 0) {
// negative implies constraint
body_vdot = body_accel_inputs[i].bottomRows(6);
body_idx = (int)(body_accel_inputs[i][0])-1;
if (!inSupport(active_supports,body_idx)) {
pdata->r->forwardJac(body_idx,orig,1,Jb);
pdata->r->forwardJacDot(body_idx,orig,1,Jbdot);
for (int j=0; j<6; j++) {
if (!std::isnan(body_vdot[j])) {
Aeq.block(equality_ind,0,1,nq) = Jb.row(j);
beq[equality_ind++] = -Jbdot.row(j)*qdvec + body_vdot[j];
}
}
}
}
}
if (num_condof>0) {
// add joint acceleration constraints
for (int i=0; i<num_condof; i++) {
Aeq(equality_ind,(int)condof[i]-1) = 1;
beq[equality_ind++] = qddot_des[(int)condof[i]-1];
}
}
int n_ineq = 2*nu+2*6*pdata->n_body_accel_bounds;
MatrixXd Ain = MatrixXd::Zero(n_ineq,nparams); // note: obvious sparsity here
VectorXd bin = VectorXd::Zero(n_ineq);
// linear input saturation constraints
// u=B_act'*(H_act*qdd + C_act - Jz_act'*z - Dbar_act*beta)
// using transpose instead of inverse because B is orthogonal
Ain.topLeftCorner(nu,nq) = pdata->B_act.transpose()*pdata->H_act;
Ain.block(0,nq,nu,nc*nd) = -pdata->B_act.transpose()*D_act;
bin.head(nu) = -pdata->B_act.transpose()*pdata->C_act + pdata->umax;
Ain.block(nu,0,nu,nparams) = -1*Ain.block(0,0,nu,nparams);
bin.segment(nu,nu) = pdata->B_act.transpose()*pdata->C_act - pdata->umin;
int body_index;
int constraint_start_index = 2*nu;
for (int i=0; i<pdata->n_body_accel_bounds; i++) {
body_index = pdata->accel_bound_body_idx[i];
pdata->r->forwardJac(body_index,orig,1,Jb);
pdata->r->forwardJacDot(body_index,orig,1,Jbdot);
Ain.block(constraint_start_index,0,6,pdata->r->num_dof) = Jb;
bin.segment(constraint_start_index,6) = -Jbdot*qdvec + pdata->max_body_acceleration[i];
constraint_start_index += 6;
Ain.block(constraint_start_index,0,6,pdata->r->num_dof) = -Jb;
bin.segment(constraint_start_index,6) = Jbdot*qdvec - pdata->min_body_acceleration[i];
constraint_start_index += 6;
}
for (int i=0; i<n_ineq; i++) {
// remove inf constraints---needed by gurobi
if (std::isinf(double(bin(i)))) {
Ain.row(i) = 0*Ain.row(i);
bin(i)=0;
}
}
GRBmodel * model = NULL;
int info=-1;
// set obj,lb,up
VectorXd lb(nparams), ub(nparams);
lb.head(nq) = qdd_lb;
ub.head(nq) = qdd_ub;
lb.segment(nq,nf) = VectorXd::Zero(nf);
ub.segment(nq,nf) = 1e3*VectorXd::Ones(nf);
lb.tail(neps) = -pdata->slack_limit*VectorXd::Ones(neps);
ub.tail(neps) = pdata->slack_limit*VectorXd::Ones(neps);
VectorXd alpha(nparams);
MatrixXd Qnfdiag(nf,1), Qneps(neps,1);
vector<MatrixXd*> QBlkDiag( nc>0 ? 3 : 1 ); // nq, nf, neps // this one is for gurobi
VectorXd w = (pdata->w_qdd.array() + REG).matrix();
#ifdef USE_MATRIX_INVERSION_LEMMA
bool include_body_accel_cost_terms = pdata->n_body_accel_inputs > 0 && pdata->body_accel_input_weights.array().maxCoeff() > 1e-10;
if (use_fast_qp > 0 && !include_angular_momentum && !include_body_accel_cost_terms)
{
// TODO: update to include angular momentum, body accel objectives.
// We want Hqp inverse, which I can compute efficiently using the
// matrix inversion lemma (see wikipedia):
// inv(A + U'CV) = inv(A) - inv(A)*U* inv([ inv(C)+ V*inv(A)*U ]) V inv(A)
if (nc>0) {
MatrixXd Wi = ((1/(pdata->w_qdd.array() + REG)).matrix()).asDiagonal();
if (R_DQyD_ls.trace()>1e-15) { // R_DQyD_ls is not zero
pdata->Hqp = Wi - Wi*Jcom.transpose()*(R_DQyD_ls.inverse() + Jcom*Wi*Jcom.transpose()).inverse()*Jcom*Wi;
}
}
else {
pdata->Hqp = MatrixXd::Constant(nq,1,1/(1+REG));
}
#ifdef TEST_FAST_QP
if (nc>0) {
MatrixXd Hqp_test(nq,nq);
MatrixXd W = w.asDiagonal();
Hqp_test = (Jcom.transpose()*R_DQyD_ls*Jcom + W).inverse();
if (((Hqp_test-pdata->Hqp).array().abs()).maxCoeff() > 1e-6) {
mexErrMsgTxt("Q submatrix inverse from matrix inversion lemma does not match direct Q inverse.");
}
}
#endif
Qnfdiag = MatrixXd::Constant(nf,1,1/REG);
Qneps = MatrixXd::Constant(neps,1,1/(.001+REG));
QBlkDiag[0] = &pdata->Hqp;
if (nc>0) {
QBlkDiag[1] = &Qnfdiag;
QBlkDiag[2] = &Qneps; // quadratic slack var cost, Q(nparams-neps:end,nparams-neps:end)=eye(neps)
}
MatrixXd Ain_lb_ub(n_ineq+2*nparams,nparams);
VectorXd bin_lb_ub(n_ineq+2*nparams);
Ain_lb_ub << Ain, // note: obvious sparsity here
-MatrixXd::Identity(nparams,nparams),
MatrixXd::Identity(nparams,nparams);
bin_lb_ub << bin, -lb, ub;
info = fastQPThatTakesQinv(QBlkDiag, f, Aeq, beq, Ain_lb_ub, bin_lb_ub, pdata->active, alpha);
//if (info<0) mexPrintf("fastQP info = %d. Calling gurobi.\n", info);
}
else {
#endif
if (nc>0) {
pdata->Hqp = Jcom.transpose()*R_DQyD_ls*Jcom;
if (include_angular_momentum) {
pdata->Hqp += pdata->Ak.transpose()*pdata->W_kdot*pdata->Ak;
}
pdata->Hqp += pdata->w_qdd.asDiagonal();
pdata->Hqp += REG*MatrixXd::Identity(nq,nq);
} else {
pdata->Hqp = (1+REG)*MatrixXd::Identity(nq,nq);
}
// add in body spatial acceleration cost terms
double w_i;
for (int i=0; i<pdata->n_body_accel_inputs; i++) {
w_i=pdata->body_accel_input_weights(i);
if (w_i > 0) {
body_vdot = body_accel_inputs[i].bottomRows(6);
body_idx = (int)(body_accel_inputs[i][0])-1;
if (!inSupport(active_supports,body_idx)) {
pdata->r->forwardJac(body_idx,orig,1,Jb);
pdata->r->forwardJacDot(body_idx,orig,1,Jbdot);
for (int j=0; j<6; j++) {
if (!std::isnan(body_vdot[j])) {
pdata->Hqp += w_i*(Jb.row(j)).transpose()*Jb.row(j);
f.head(nq) += w_i*(qdvec.transpose()*Jbdot.row(j).transpose() - body_vdot[j])*Jb.row(j).transpose();
}
}
}
}
}
Qnfdiag = MatrixXd::Constant(nf,1,pdata->w_grf+REG);
Qneps = MatrixXd::Constant(neps,1,pdata->w_slack+REG);
QBlkDiag[0] = &pdata->Hqp;
if (nc>0) {
QBlkDiag[1] = &Qnfdiag;
QBlkDiag[2] = &Qneps; // quadratic slack var cost, Q(nparams-neps:end,nparams-neps:end)=eye(neps)
}
MatrixXd Ain_lb_ub(n_ineq+2*nparams,nparams);
VectorXd bin_lb_ub(n_ineq+2*nparams);
Ain_lb_ub << Ain, // note: obvious sparsity here
-MatrixXd::Identity(nparams,nparams),
MatrixXd::Identity(nparams,nparams);
bin_lb_ub << bin, -lb, ub;
if (use_fast_qp > 0)
{ // set up and call fastqp
info = fastQP(QBlkDiag, f, Aeq, beq, Ain_lb_ub, bin_lb_ub, pdata->active, alpha);
//if (info<0) mexPrintf("fastQP info=%d... calling Gurobi.\n", info);
}
else {
// use gurobi active set
model = gurobiActiveSetQP(pdata->env,QBlkDiag,f,Aeq,beq,Ain,bin,lb,ub,pdata->vbasis,pdata->vbasis_len,pdata->cbasis,pdata->cbasis_len,alpha);
CGE(GRBgetintattr(model,"NumVars",&pdata->vbasis_len), pdata->env);
CGE(GRBgetintattr(model,"NumConstrs",&pdata->cbasis_len), pdata->env);
info=66;
//info = -1;
}
if (info<0) {
model = gurobiQP(pdata->env,QBlkDiag,f,Aeq,beq,Ain,bin,lb,ub,pdata->active,alpha);
int status; CGE(GRBgetintattr(model, "Status", &status), pdata->env);
//if (status!=2) mexPrintf("Gurobi reports non-optimal status = %d\n", status);
}
#ifdef USE_MATRIX_INVERSION_LEMMA
}
#endif
//----------------------------------------------------------------------
// Solve for inputs ----------------------------------------------------
VectorXd y(nu);
VectorXd qdd = alpha.head(nq);
VectorXd beta = alpha.segment(nq,nc*nd);
// use transpose because B_act is orthogonal
y = pdata->B_act.transpose()*(pdata->H_act*qdd + pdata->C_act - D_act*beta);
//y = pdata->B_act.jacobiSvd(ComputeThinU|ComputeThinV).solve(pdata->H_act*qdd + pdata->C_act - Jz_act.transpose()*lambda - D_act*beta);
if (nlhs>0) {
plhs[0] = eigenToMatlab(y);
}
if (nlhs>1) {
plhs[1] = eigenToMatlab(qdd);
}
if (nlhs>2) {
plhs[2] = mxCreateNumericMatrix(1,1,mxINT32_CLASS,mxREAL);
memcpy(mxGetData(plhs[2]),&info,sizeof(int));
}
if (nlhs>3) {
plhs[3] = mxCreateDoubleMatrix(1,active_supports.size(),mxREAL);
pr = mxGetPr(plhs[3]);
int i=0;
for (vector<SupportStateElement>::iterator iter = active_supports.begin(); iter!=active_supports.end(); iter++) {
pr[i++] = (double) (iter->body_idx + 1);
}
}
if (nlhs>4) {
plhs[4] = eigenToMatlab(alpha);
}
if (nlhs>5) {
plhs[5] = eigenToMatlab(pdata->Hqp);
}
if (nlhs>6) {
plhs[6] = eigenToMatlab(f);
}
if (nlhs>7) {
plhs[7] = eigenToMatlab(Aeq);
}
if (nlhs>8) {
plhs[8] = eigenToMatlab(beq);
}
if (nlhs>9) {
plhs[9] = eigenToMatlab(Ain_lb_ub);
}
if (nlhs>10) {
plhs[10] = eigenToMatlab(bin_lb_ub);
}
if (nlhs>11) {
plhs[11] = eigenToMatlab(Qnfdiag);
}
if (nlhs>12) {
plhs[12] = eigenToMatlab(Qneps);
}
if (nlhs>13) {
double Vdot;
if (nc>0)
// note: Sdot is 0 for ZMP/double integrator dynamics, so we omit that term here
Vdot = ((2*x_bar.transpose()*S + s1.transpose())*(A_ls*x_bar + B_ls*(Jcomdot*qdvec + Jcom*qdd)) + s1dot.transpose()*x_bar)(0) + s2dot;
else
Vdot = 0;
plhs[13] = mxCreateDoubleScalar(Vdot);
}
if (nlhs>14) {
RigidBodyManipulator* r = pdata->r;
VectorXd individual_cops = individualSupportCOPs(r, active_supports, normals, B, beta);
plhs[14] = eigenToMatlab(individual_cops);
}
if (model) {
GRBfreemodel(model);
}
// GRBfreeenv(env);
}
void MyWindow::setState(const VectorXd& newState) {
mDofVels = newState.tail(mDofVels.size());
mDofs = newState.head(mDofs.size());
}
double DescriptorDist( const VectorXd& m, const VectorXd& t, int neighbours )
{
const int dsize = std::min((int)m.rows(),neighbours);
return ( m.head(dsize) - t.head(dsize) ).norm();
}
void ImplicitEuler::renderNewtonsMethod(VectorXd& ext_force){
//Implicit Code
v_k.setZero();
x_k.setZero();
x_k = x_old;
v_k = v_old;
int ignorePastIndex = TV.rows() - fixedVerts.size();
SparseMatrix<double> forceGradientStaticBlock;
forceGradientStaticBlock.resize(3*ignorePastIndex, 3*ignorePastIndex);
SparseMatrix<double> RegMassBlock;
RegMassBlock.resize(3*ignorePastIndex, 3*ignorePastIndex);
RegMassBlock = RegMass.block(0, 0, 3*ignorePastIndex, 3*ignorePastIndex);
forceGradient.setZero();
bool Nan=false;
int NEWTON_MAX = 10, i =0;
// cout<<"--------"<<simTime<<"-------"<<endl;
// cout<<"x_k"<<endl;
// cout<<x_k<<endl<<endl;
// cout<<"v_k"<<endl;
// cout<<v_k<<endl<<endl;
// cout<<"--------------------"<<endl;
for( i=0; i<NEWTON_MAX; i++){
grad_g.setZero();
ImplicitXtoTV(x_k, TVk);//TVk value changed in function
ImplicitCalculateElasticForceGradient(TVk, forceGradient);
ImplicitCalculateForces(TVk, forceGradient, x_k, f);
f = f+ext_force;
// VectorXd g_block = x_k - x_old -h*v_old -h*h*InvMass*f;
// grad_g = Ident - h*h*InvMass*forceGradient - h*rayleighCoeff*InvMass*forceGradient;
//Block forceGrad and f to exclude the fixed verts
forceGradientStaticBlock = forceGradient.block(0,0, 3*(ignorePastIndex), 3*ignorePastIndex);
VectorXd g = RegMass*x_k - RegMass*x_old - h*RegMass*v_old - h*h*f;
VectorXd g_block = g.head(ignorePastIndex*3);
grad_g = RegMassBlock - h*h*forceGradientStaticBlock - h*rayleighCoeff*forceGradientStaticBlock;
//solve for delta x
// Conj Grad
// ConjugateGradient<SparseMatrix<double>> cg;
// cg.compute(grad_g);
// VectorXd deltaX = -1*cg.solve(g);
// Sparse Cholesky LL^T
SimplicialLLT<SparseMatrix<double>> llt;
llt.compute(grad_g);
if(llt.info() == Eigen::NumericalIssue){
cout<<"Possibly using a non- pos def matrix in the LLT method"<<endl;
exit(0);
}
VectorXd deltaX = -1* llt.solve(g_block);
x_k.segment(0, 3*(ignorePastIndex)) += deltaX;
//Sparse QR
// SparseQR<SparseMatrix<double>, COLAMDOrdering<int>> sqr;
// sqr.compute(grad_g);
// VectorXd deltaX = -1*sqr.solve(g_block);
// x_k.segment(0, 3*(ignorePastIndex)) += deltaX;
// CholmodSimplicialLLT<SparseMatrix<double>> cholmodllt;
// cholmodllt.compute(grad_g);
// VectorXd deltaX = -cholmodllt.solve(g_block);
if(x_k != x_k){
Nan = true;
break;
}
if(g_block.squaredNorm()<.00000001){
break;
}
}
if(Nan){
cout<<"ERROR: Newton's method doesn't converge"<<endl;
cout<<i<<endl;
exit(0);
}
if(i== NEWTON_MAX){
cout<<"ERROR: Newton max reached"<<endl;
cout<<i<<endl;
exit(0);
}
v_old.setZero();
v_old = (x_k - x_old)/h;
x_old = x_k;
}
double drwnSparseLPSolver::solve()
{
DRWN_FCN_TIC;
const int n = _A.cols();
const int m = _A.rows();
// find feasible starting point
if (_x.rows() == 0) {
_x = VectorXd::Zero(n);
for (int i = 0; i < n; i++) {
if (isUnbounded(i)) continue;
if (_ub[i] == DRWN_DBL_MAX) {
_x[i] = _lb[i] + 1.0;
} else if (_lb[i] == -DRWN_DBL_MAX) {
_x[i] = _ub[i] - 1.0;
} else {
_x[i] = 0.5 * (_lb[i] + _ub[i]);
}
}
}
// initialize kkt system variables
SparseMatrix<double> F(n + m, n + m);
VectorXd g = VectorXd::Zero(n + m);
vector<Eigen::Triplet<double> > entries;
entries.reserve(n + 2 * _A.nonZeros());
for (int i = 0; i < n; i++) {
entries.push_back(Eigen::Triplet<double>(i, i, 1.0));
}
for (int k = 0; k < _A.outerSize(); k++) {
for (SparseMatrix<double>::InnerIterator it(_A, k); it; ++it) {
entries.push_back(Eigen::Triplet<double>(n + it.row(), it.col(), it.value()));
entries.push_back(Eigen::Triplet<double>(it.col(), n + it.row(), it.value()));
}
}
F.setFromTriplets(entries.begin(), entries.end());
// initialize dual variables (if needed)
VectorXd nu = VectorXd::Zero(m);
// iterate on interior point method
double t = drwnLPSolver::t0;
while (1) {
// determine feasibility
const bool bFeasible = ((_b - _A * _x).squaredNorm() < drwnLPSolver::eps);
if (!bFeasible) {
DRWN_LOG_VERBOSE("...finding feasible point");
}
// centering step and update
for (unsigned iter = 0; iter < drwnLPSolver::maxiters; iter++) {
//! \todo solve with blockwise elimination
// construct KKT system, Fx = g
// | H A^T | | dx | = | - g |
// | A 0 | | w | | b - Ax |
for (int i = 0; i < n; i++) {
F.coeffRef(i, i) = 0.0;
if (_ub[i] != DRWN_DBL_MAX) {
F.coeffRef(i, i) += 1.0 / ((_ub[i] - _x[i]) * (_ub[i] - _x[i]));
}
if (_lb[i] != -DRWN_DBL_MAX) {
F.coeffRef(i, i) += 1.0 / ((_x[i] - _lb[i]) * (_x[i] - _lb[i]));
}
}
for (int i = 0; i < n; i++) {
g[i] = - t * _c[i];
if (_ub[i] != DRWN_DBL_MAX) {
g[i] += 1.0 / (_x[i] - _ub[i]);
}
if (_lb[i] != -DRWN_DBL_MAX) {
g[i] += 1.0 / (_x[i] - _lb[i]);
}
}
g.tail(m) = _b - _A * _x;
// check terminating condition
const double r_primal = g.tail(m).squaredNorm();
const double r_dual = (_A.transpose() * nu - g.head(n)).squaredNorm();
//if (!bFeasible && (r_primal + r_dual < drwnLPSolver::eps)) break;
if (!bFeasible && (r_primal < drwnLPSolver::eps)) break;
// solve KKT system
SimplicialLDLT<SparseMatrix<double> > chol(F);
const VectorXd dxnu = chol.solve(g);
const double lambda_sqr = g.head(n).dot(dxnu.head(n));
if (bFeasible && (0.5 * lambda_sqr < 1.0e-6)) break;
if (bFeasible) {
// feasible line search
const double f_prev = t * _c.dot(_x) + barrierFunction(_x);
const double delta_f = drwnLPSolver::alpha * g.dot(dxnu.head(n));
double step = 1.0;
while (1) {
const VectorXd nx = _x + step * dxnu.head(n);
if (isWithinBounds(nx)) {
const double f = t * _c.dot(nx) + barrierFunction(nx);
if (f - f_prev < step * delta_f) {
_x = nx;
break;
}
}
step *= drwnLPSolver::beta;
}
} else {
// infeasible start line search
DRWN_LOG_DEBUG("...iteration " << iter << ", primal residual " << r_primal
<< ", dual residual " << r_dual);
double step = 1.0;
while (1) {
const VectorXd nx = _x + step * dxnu.head(n);
if (isWithinBounds(nx)) {
const VectorXd nnu = (1.0 - step) * nu + step * dxnu.tail(m);
const double r = (_A.transpose() * nnu - g.head(n)).squaredNorm() +
(_b - _A * nx).squaredNorm();
if (r <= (1.0 - drwnLPSolver::alpha * step) * (r_primal + r_dual)) {
_x = nx;
nu = nnu;
break;
}
}
step *= drwnLPSolver::beta;
}
}
}
// check if feasible point was found
if (!bFeasible && ((_b - _A * _x).squaredNorm() > drwnLPSolver::eps)) {
DRWN_LOG_WARNING("...could not find a feasible point (residual norm is " <<
(_b - _A * _x).norm() << ")");
DRWN_FCN_TOC;
return DRWN_DBL_MAX;
}
DRWN_LOG_VERBOSE("...objective is " << _c.dot(_x));
// check stopping criteria
if (m < drwnLPSolver::eps * t) break;
// update barrier function multiplier
t *= drwnLPSolver::mu;
}
// compute true objective and return
DRWN_FCN_TOC;
return _c.dot(_x);
}
bool solve(const int mode)
{
bool batch = (mode !=2 || first_ordered_node_ == 0);
bool order = (mode !=0 && n_nodes_ > 1);
// BATCH
if (batch)
{
// REORDER
if (order)
ordering(0);
//print_problem();
// SOLVE
t_solving_ = clock();
A_.makeCompressed();
solver_.compute(A_);
if (solver_.info() != Success)
{
std::cout << "decomposition failed" << std::endl;
return 0;
}
x_incr_ = solver_.solve(b_);
R_ = solver_.matrixR();
//std::cout << "R" << std::endl << MatrixXd::Identity(R_.cols(), R_.cols()) * R_ << std::endl;
time_solving_ += ((double) clock() - t_solving_) / CLOCKS_PER_SEC;
}
// INCREMENTAL
else
{
// REORDER SUBPROBLEM
ordering(first_ordered_node_);
//print_problem();
// SOLVE ORDERED SUBPROBLEM
t_solving_= clock();
A_nodes_.makeCompressed();
A_.makeCompressed();
// finding measurements block
SparseMatrix<int> measurements_to_initial = A_nodes_.col(first_ordered_node_);
// std::cout << "measurements_to_initial " << measurements_to_initial << std::endl;
// std::cout << "measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]] " << measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]] << std::endl;
int first_ordered_measurement = measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]];
int ordered_measurements = A_.rows() - measurements_.at(first_ordered_measurement).location;
int ordered_variables = A_.cols() - nodes_.at(first_ordered_node_).location;
int unordered_variables = nodes_.at(first_ordered_node_).location;
SparseMatrix<double, ColMajor> A_partial = A_.bottomRightCorner(ordered_measurements, ordered_variables);
solver_.compute(A_partial);
if (solver_.info() != Success)
{
std::cout << "decomposition failed" << std::endl;
return 0;
}
//std::cout << "R new" << std::endl << MatrixXd::Identity(A_partial.cols(), A_partial.cols()) * solver_.matrixR() << std::endl;
x_incr_.tail(ordered_variables) = solver_.solve(b_.tail(ordered_measurements));
// store new part of R
eraseSparseBlock(R_, unordered_variables, unordered_variables, ordered_variables, ordered_variables);
//std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl;
addSparseBlock(solver_.matrixR(), R_, unordered_variables, unordered_variables);
//std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl;
R_.makeCompressed();
// solving not ordered subproblem
if (unordered_variables > 0)
{
//std::cout << "--------------------- solving unordered part" << std::endl;
SparseMatrix<double, ColMajor> R1 = R_.topLeftCorner(unordered_variables, unordered_variables);
//std::cout << "R1" << std::endl << MatrixXd::Identity(R1.rows(), R1.rows()) * R1 << std::endl;
SparseMatrix<double, ColMajor> R2 = R_.topRightCorner(unordered_variables, ordered_variables);
//std::cout << "R2" << std::endl << MatrixXd::Identity(R2.rows(), R2.rows()) * R2 << std::endl;
solver_.compute(R1);
if (solver_.info() != Success)
{
std::cout << "decomposition failed" << std::endl;
return 0;
}
x_incr_.head(unordered_variables) = solver_.solve(b_.head(unordered_variables) - R2 * x_incr_.tail(ordered_variables));
}
}
// UNDO ORDERING FOR RESULT
PermutationMatrix<Dynamic, Dynamic, int> acc_permutation(A_.cols());
nodePermutation2VariablesPermutation(acc_node_permutation_, acc_permutation); // TODO via pointers
x_incr_ = acc_permutation.inverse() * x_incr_;
time_solving_ += ((double) clock() - t_solving_) / CLOCKS_PER_SEC;
return 1;
}
double drwnLPSolver::solve()
{
DRWN_FCN_TIC;
const int n = _A.cols();
const int m = _A.rows();
// find feasible starting point
if (_x.rows() == 0) {
_x = VectorXd::Zero(n);
for (int i = 0; i < n; i++) {
if (isUnbounded(i)) continue;
if (_ub[i] == DRWN_DBL_MAX) {
_x[i] = _lb[i] + 1.0;
} else if (_lb[i] == -DRWN_DBL_MAX) {
_x[i] = _ub[i] - 1.0;
} else {
_x[i] = 0.5 * (_lb[i] + _ub[i]);
}
}
}
// initialize kkt system variables
MatrixXd F = MatrixXd::Zero(n + m, n + m);
VectorXd g = VectorXd::Zero(n + m);
F.block(n, 0, m, n) = _A;
F.block(0, n, n, m) = _A.transpose();
// initialize dual variables (if needed)
VectorXd nu = VectorXd::Zero(m);
// iterate on interior point method
double t = t0;
while (1) {
// determine feasibility
const bool bFeasible = ((_b - _A * _x).squaredNorm() < eps);
if (!bFeasible) {
DRWN_LOG_VERBOSE("...finding feasible point");
}
// centering step and update
for (unsigned iter = 0; iter < maxiters; iter++) {
//! \todo solve with blockwise elimination
// construct KKT system, Fx = g
// | H A^T | | dx | = | - g |
// | A 0 | | w | | b - Ax |
for (int i = 0; i < n; i++) {
F(i, i) = 0.0;
if (_ub[i] != DRWN_DBL_MAX) {
F(i, i) += 1.0 / ((_ub[i] - _x[i]) * (_ub[i] - _x[i]));
}
if (_lb[i] != -DRWN_DBL_MAX) {
F(i, i) += 1.0 / ((_x[i] - _lb[i]) * (_x[i] - _lb[i]));
}
}
for (int i = 0; i < n; i++) {
g[i] = - t * _c[i];
if (_ub[i] != DRWN_DBL_MAX) {
g[i] += 1.0 / (_x[i] - _ub[i]);
}
if (_lb[i] != -DRWN_DBL_MAX) {
g[i] += 1.0 / (_x[i] - _lb[i]);
}
}
g.tail(m) = _b - _A * _x;
// check terminating condition
const double r_primal = g.tail(m).squaredNorm();
const double r_dual = (_A.transpose() * nu - g.head(n)).squaredNorm();
//if (!bFeasible && (r_primal + r_dual < drwnLPSolver::eps)) break;
if (!bFeasible && (r_primal < drwnLPSolver::eps)) break;
// solve KKT system
const VectorXd dxnu = F.fullPivLu().solve(g);
const double lambda_sqr = g.head(n).dot(dxnu.head(n));
if (bFeasible && (0.5 * lambda_sqr < 1.0e-6)) break;
if (bFeasible) {
// feasible line search
const double f_prev = t * _c.dot(_x) + barrierFunction(_x);
const double delta_f = alpha * g.dot(dxnu.head(n));
double step = 1.0;
while (1) {
const VectorXd nx = _x + step * dxnu.head(n);
if (isWithinBounds(nx)) {
const double f = t * _c.dot(nx) + barrierFunction(nx);
if (f - f_prev < step * delta_f) {
_x = nx;
break;
}
}
step *= beta;
}
} else {
// infeasible start line search
double step = 1.0;
while (1) {
const VectorXd nx = _x + step * dxnu.head(n);
if (isWithinBounds(nx)) {
const VectorXd nnu = (1.0 - step) * nu + step * dxnu.tail(m);
const double r = (_A.transpose() * nnu - g.head(n)).squaredNorm() +
(_b - _A * nx).squaredNorm();
if (r <= (1.0 - alpha * step) * (r_primal + r_dual)) {
_x = nx;
nu = nnu;
break;
}
}
step *= beta;
}
}
}
// check if feasible point was found
if (!bFeasible && ((_b - _A * _x).squaredNorm() > eps)) {
DRWN_LOG_WARNING("...could not find a feasible point (residual norm is " <<
(_b - _A * _x).norm() << ")");
DRWN_FCN_TOC;
return DRWN_DBL_MAX;
}
DRWN_LOG_VERBOSE("...objective is " << _c.dot(_x));
// check stopping criteria
if (m < eps * t) break;
// update barrier function multiplier
t *= mu;
}
// compute true objective and return
DRWN_FCN_TOC;
return _c.dot(_x);
}
MatrixXd SMRFilter::expandingTPS(MatrixXd cx, MatrixXd cy, MatrixXd cz)
{
log()->get(LogLevel::Info) << "TPS: Reticulating splines...\n";
MatrixXd S = cz;
int num_nan_detect(0);
int num_nan_replace(0);
for (auto outer_col = 0; outer_col < m_numCols; ++outer_col)
{
for (auto outer_row = 0; outer_row < m_numRows; ++outer_row)
{
if (!std::isnan(S(outer_row, outer_col)))
continue;
num_nan_detect++;
// Further optimizations are achieved by estimating only the
// interpolated surface within a local neighbourhood (e.g. a 7 x 7
// neighbourhood is used in our case) of the cell being filtered.
int radius = 3;
bool solution = false;
while (!solution)
{
// std::cerr << radius;
int cs = Utils::clamp(outer_col-radius, 0, m_numCols-1);
int ce = Utils::clamp(outer_col+radius, 0, m_numCols-1);
int col_size = ce - cs + 1;
int rs = Utils::clamp(outer_row-radius, 0, m_numRows-1);
int re = Utils::clamp(outer_row+radius, 0, m_numRows-1);
int row_size = re - rs + 1;
MatrixXd Xn = cx.block(rs, cs, row_size, col_size);
MatrixXd Yn = cy.block(rs, cs, row_size, col_size);
MatrixXd Hn = cz.block(rs, cs, row_size, col_size);
int nsize = Hn.size();
VectorXd T = VectorXd::Zero(nsize);
MatrixXd P = MatrixXd::Zero(nsize, 3);
MatrixXd K = MatrixXd::Zero(nsize, nsize);
int numK(0);
for (auto id = 0; id < Hn.size(); ++id)
{
double xj = Xn(id);
double yj = Yn(id);
double zj = Hn(id);
if (std::isnan(zj))
continue;
numK++;
T(id) = zj;
P.row(id) << 1, xj, yj;
for (auto id2 = 0; id2 < Hn.size(); ++id2)
{
if (id == id2)
continue;
double xk = Xn(id2);
double yk = Yn(id2);
double rsqr = (xj - xk) * (xj - xk) + (yj - yk) * (yj - yk);
if (rsqr == 0.0)
continue;
K(id, id2) = rsqr * std::log10(std::sqrt(rsqr));
}
}
// if (numK < 20)
// continue;
MatrixXd A = MatrixXd::Zero(nsize+3, nsize+3);
A.block(0,0,nsize,nsize) = K;
A.block(0,nsize,nsize,3) = P;
A.block(nsize,0,3,nsize) = P.transpose();
VectorXd b = VectorXd::Zero(nsize+3);
b.head(nsize) = T;
VectorXd x = A.fullPivHouseholderQr().solve(b);
Vector3d a = x.tail(3);
VectorXd w = x.head(nsize);
double sum = 0.0;
double xi2 = cx(outer_row, outer_col);
double yi2 = cy(outer_row, outer_col);
for (auto j = 0; j < nsize; ++j)
{
double xj = Xn(j);
double yj = Yn(j);
double rsqr = (xj - xi2) * (xj - xi2) + (yj - yi2) * (yj - yi2);
if (rsqr == 0.0)
continue;
sum += w(j) * rsqr * std::log10(std::sqrt(rsqr));
}
double val = a(0) + a(1)*xi2 + a(2)*yi2 + sum;
solution = !std::isnan(val);
if (!solution)
{
std::cerr << "..." << radius << std::endl;;
++radius;
continue;
}
S(outer_row, outer_col) = val;
num_nan_replace++;
// std::cerr << std::endl;
// std::cerr << std::fixed;
// std::cerr << std::setprecision(3)
// << std::left
// << "S(" << outer_row << "," << outer_col << "): "
// << std::setw(10)
// << S(outer_row, outer_col)
// // << std::setw(3)
// // << "\tz: "
// // << std::setw(10)
// // << zi
// // << std::setw(7)
// // << "\tzdiff: "
// // << std::setw(5)
// // << zi - S(outer_row, outer_col)
// // << std::setw(7)
// // << "\txdiff: "
// // << std::setw(5)
// // << xi2 - xi
// // << std::setw(7)
// // << "\tydiff: "
// // << std::setw(5)
// // << yi2 - yi
// << std::setw(7)
// << "\t# pts: "
// << std::setw(3)
// << nsize
// << std::setw(5)
// << "\tsum: "
// << std::setw(10)
// << sum
// << std::setw(9)
// << "\tw.sum(): "
// << std::setw(5)
// << w.sum()
// << std::setw(6)
// << "\txsum: "
// << std::setw(5)
// << w.dot(P.col(1))
// << std::setw(6)
// << "\tysum: "
// << std::setw(5)
// << w.dot(P.col(2))
// << std::setw(3)
// << "\ta: "
// << std::setw(8)
// << a.transpose()
// << std::endl;
}
}
}
double frac = static_cast<double>(num_nan_replace);
frac /= static_cast<double>(num_nan_detect);
log()->get(LogLevel::Info) << "TPS: Filled " << num_nan_replace << " of "
<< num_nan_detect << " holes ("
<< frac * 100.0 << "%)\n";
return S;
}