//*************************************************************************************************************************
std::string wholeBodyReach::jointToString(const Eigen::VectorXd &j, int precision)
{
if(j.size()!=ICUB_DOFS && j.size()!=ICUB_DOFS+6)
cout<<"Error in size of joint vector: "<<j.size()<<endl;
int index=0;
string ret = "";
char tmp[350];
if(j.size()==ICUB_DOFS+6)
{
ret += "base(";
for(int i=0;i<6;i++)
{
sprintf(tmp, "% .*lf ", precision, j(index));
ret+=tmp;
index++;
}
ret = ret.substr(0, ret.length()-1); // remove the last character (tab)
ret += ")\t";
}
ret += "torso(";
for(int i=0;i<3;i++)
{
sprintf(tmp, "% .*lf ", precision, j(index));
ret+=tmp;
index++;
}
ret = ret.substr(0, ret.length()-1); // remove the last character (tab)
ret += ")\tl_arm(";
for(int i=0;i<5;i++)
{
sprintf(tmp, "% .*lf ", precision, j(index));
ret+=tmp;
index++;
}
ret = ret.substr(0, ret.length()-1); // remove the last character (tab)
ret += ")\tr_arm(";
for(int i=0;i<5;i++)
{
sprintf(tmp, "% .*lf ", precision, j(index));
ret+=tmp;
index++;
}
ret = ret.substr(0, ret.length()-1); // remove the last character (tab)
ret += ")\tl_leg(";
for(int i=0;i<6;i++)
{
sprintf(tmp, "% .*lf ", precision, j(index));
ret+=tmp;
index++;
}
ret = ret.substr(0, ret.length()-1); // remove the last character (tab)
ret += ")\tr_leg(";
for(int i=0;i<6;i++)
{
sprintf(tmp, "% .*lf ", precision, j(index));
ret+=tmp;
index++;
}
ret += ")";
return ret;
}
base::PlannerStatus BFRRT::solve(
const base::PlannerTerminationCondition &ptc)
{
checkValidity();
base::GoalSampleableRegion *goal =
dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
if (!goal)
{
OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE;
}
TreeGrowingInfo tgi;
while (const base::State *st = pis_.nextStart())
{
Motion *motion = new Motion(si_);
si_->copyState(motion->state, st);
motion->root = motion->state;
tStart_->add(motion);
tgi.last_s = motion;
}
if (tStart_->size() == 0)
{
OMPL_ERROR("%s: Motion planning start tree could not be initialized!",
getName().c_str());
return base::PlannerStatus::INVALID_START;
}
if (!goal->couldSample())
{
OMPL_ERROR("%s: Insufficient states in sampleable goal region",
getName().c_str());
return base::PlannerStatus::INVALID_GOAL;
}
if (!sampler_) sampler_ = si_->allocStateSampler();
OMPL_INFORM(
"%s: Starting planning with %d states already in datastructure",
getName().c_str(), (int )(tStart_->size() + tGoal_->size()));
Motion *rmotion = new Motion(si_);
bool startTree = true;
bool solved = false;
while (ptc == false)
{
TreeData &tree = startTree ? tStart_ : tGoal_;
tgi.start = startTree;
startTree = !startTree;
TreeData &otherTree = startTree ? tStart_ : tGoal_;
if (tGoal_->size() == 0
|| pis_.getSampledGoalsCount() < tGoal_->size() / 2)
{
const base::State *st =
tGoal_->size() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
if (st)
{
Motion *motion = new Motion(si_);
si_->copyState(motion->state, st);
motion->root = motion->state;
tGoal_->add(motion);
tgi.last_g = motion;
}
if (tGoal_->size() == 0)
{
OMPL_ERROR("%s: Unable to sample any valid states for goal tree",
getName().c_str());
break;
}
}
static double nearest_r = 0.05
* distanceFunction(tgi.last_s, tgi.last_g);
/// Get a random state
bool r_ok = false;
do
{
sampler_->sampleUniform(rmotion->state);
r_ok = si_->getStateValidityChecker()->isValid(rmotion->state);
} while (!r_ok && ptc == false);
Motion *nearest_s = tStart_->nearest(rmotion);
Motion *nearest_g = tGoal_->nearest(rmotion);
Motion *last_valid_motion = new Motion(si_);
std::pair<ompl::base::State*, double> last_valid(
last_valid_motion->state, 0);
Motion *new_s = NULL;
Motion *new_g = NULL;
/// Connect random node to start tree
bool succ_s = si_->checkMotion(nearest_s->state, rmotion->state,
last_valid);
if (succ_s)
{
Motion *motion = new Motion(si_);
si_->copyState(motion->state, rmotion->state);
motion->parent = nearest_s;
tStart_->add(motion);
new_s = motion;
}
else
{
if (last_valid.second == 0) last_valid_motion->state = NULL;
Eigen::VectorXd eigen_g((int) si_->getStateDimension());
memcpy(eigen_g.data(),
rmotion->state->as<ompl::base::RealVectorStateSpace::StateType>()->values,
sizeof(double) * eigen_g.rows());
local_map_->jointRef = eigen_g;
local_solver_->getProblem()->setTau(1e-4);
Motion *new_motion = new Motion(si_);
if (localSolve(nearest_s, last_valid_motion->state, new_motion))
{
new_s = new_motion;
tStart_->add(new_motion);
succ_s = true;
}
else if (new_motion->internal_path)
{
si_->copyState(rmotion->state, new_motion->state);
bool addNew = true;
// std::vector<Motion*> n_motions;
// tStart_->nearestR(new_motion, nearest_r, n_motions);
// for (int i = 0; i < n_motions.size(); i++)
// if (!n_motions[i]->global_valid_)
// {
// addNew = false;
// break;
// }
if (addNew)
{
new_motion->global_valid_ = false;
tStart_->add(new_motion);
si_->copyState(rmotion->state, new_motion->state);
}
}
}
/// For goal tree, do the same thing
last_valid.second = 0;
bool succ_g = si_->checkMotion(nearest_g->state, rmotion->state,
last_valid);
if (succ_g)
{
Motion *motion = new Motion(si_);
si_->copyState(motion->state, rmotion->state);
motion->parent = nearest_g;
tGoal_->add(motion);
new_g = motion;
}
else
{
if (last_valid.second == 0) last_valid_motion->state = NULL;
Eigen::VectorXd eigen_g((int) si_->getStateDimension());
memcpy(eigen_g.data(),
rmotion->state->as<ompl::base::RealVectorStateSpace::StateType>()->values,
sizeof(double) * eigen_g.rows());
local_map_->jointRef = eigen_g;
local_solver_->getProblem()->setTau(1e-4);
Motion *new_motion = new Motion(si_);
if (localSolve(nearest_g, last_valid_motion->state, new_motion))
{
new_g = new_motion;
succ_g = true;
}
else if (new_motion->internal_path)
{
si_->copyState(rmotion->state, new_motion->state);
bool addNew = true;
// std::vector<Motion*> n_motions;
// tGoal_->nearestR(new_motion, nearest_r, n_motions);
// for (int i = 0; i < n_motions.size(); i++)
// if (!n_motions[i]->global_valid_)
// {
// addNew = false;
// break;
// }
if (addNew)
{
new_motion->global_valid_ = false;
tGoal_->add(new_motion);
}
}
}
/// If succeeded both ways, the a solution is found
if (succ_s && succ_g)
{
connectionPoint_ = std::make_pair(new_s->state, new_g->state);
Motion *solution = new_s;
std::vector<Motion*> mpath1;
while (solution != NULL)
{
if (solution->internal_path != nullptr)
{
for (int i = solution->internal_path->rows() - 1; i > 0; i--)
{
Motion *local_motion = new Motion(si_);
Eigen::VectorXd tmp = solution->internal_path->row(i);
memcpy(
local_motion->state->as<
ompl::base::RealVectorStateSpace::StateType>()->values,
tmp.data(),
sizeof(double) * (int) si_->getStateDimension());
mpath1.push_back(local_motion);
}
if (solution->inter_state != NULL)
{
Motion *local_motion = new Motion(si_);
si_->copyState(local_motion->state, solution->inter_state);
mpath1.push_back(local_motion);
}
}
else
mpath1.push_back(solution);
solution = solution->parent;
}
solution = new_g;
std::vector<Motion*> mpath2;
while (solution != NULL)
{
if (solution->internal_path != nullptr)
{
for (int i = solution->internal_path->rows() - 1; i > 0; i--)
{
Motion *local_motion = new Motion(si_);
Eigen::VectorXd tmp = solution->internal_path->row(i);
memcpy(
local_motion->state->as<
ompl::base::RealVectorStateSpace::StateType>()->values,
tmp.data(),
sizeof(double) * (int) si_->getStateDimension());
mpath2.push_back(local_motion);
}
if (solution->inter_state != NULL)
{
Motion *local_motion = new Motion(si_);
si_->copyState(local_motion->state, solution->inter_state);
mpath2.push_back(local_motion);
}
}
else
mpath2.push_back(solution);
solution = solution->parent;
}
PathGeometric *path = new PathGeometric(si_);
path->getStates().reserve(mpath1.size() + mpath2.size());
for (int i = mpath1.size() - 1; i >= 0; --i)
path->append(mpath1[i]->state);
for (unsigned int i = 0; i < mpath2.size(); ++i)
path->append(mpath2[i]->state);
pdef_->addSolutionPath(base::PathPtr(path), false, 0.0, getName());
solved = true;
break;
}
}
si_->freeState(rmotion->state);
delete rmotion;
OMPL_INFORM("%s: Created %u states (%u start + %u goal)",
getName().c_str(), tStart_->size() + tGoal_->size(), tStart_->size(),
tGoal_->size());
return
solved ?
base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT;
}
RcppExport SEXP svds_gen(SEXP A_mat_r, SEXP m_scalar_r, SEXP n_scalar_r,
SEXP k_scalar_r, SEXP nu_scalar_r, SEXP nv_scalar_r,
SEXP params_list_r, SEXP mattype_scalar_r)
{
BEGIN_RCPP
Rcpp::List params_svds(params_list_r);
int m = as<int>(m_scalar_r);
int n = as<int>(n_scalar_r);
int k = as<int>(k_scalar_r);
int nu = as<int>(nu_scalar_r);
int nv = as<int>(nv_scalar_r);
int ncv = as<int>(params_svds["ncv"]);
double tol = as<double>(params_svds["tol"]);
int maxitr = as<int>(params_svds["maxitr"]);
int mattype = as<int>(mattype_scalar_r);
// Operation for original matrix
MatProd* op_orig = get_mat_prod_op(A_mat_r, m, n, params_list_r, mattype);
// Operation for SVD
MatProd* op;
if(m > n)
op = new SVDTallOp(op_orig);
else
op = new SVDWideOp(op_orig);
SymEigsSolver<double, LARGEST_ALGE, MatProd> eigs(op, k, ncv);
eigs.init();
int nconv = eigs.compute(maxitr, tol);
if(nconv < k)
Rcpp::warning("only %d singular values converged, less than k = %d", nconv, k);
nu = std::min(nu, nconv);
nv = std::min(nv, nconv);
Eigen::VectorXd evals = eigs.eigenvalues();
Eigen::MatrixXd evecs = eigs.eigenvectors(std::max(nu, nv));
Rcpp::NumericVector d(nconv);
Rcpp::NumericMatrix u(m, nu), v(n, nv);
int nops = 0;
// Copy evals to d and take the square root
std::copy(evals.data(), evals.data() + nconv, d.begin());
std::transform(d.begin(), d.end(), d.begin(), simple_sqrt);
// Copy evecs to u or v according to the shape of A
// If A is tall, copy evecs to v, otherwise copy to u
if(m > n)
std::copy(evecs.data(), evecs.data() + nv * n, v.begin());
else
std::copy(evecs.data(), evecs.data() + nu * m, u.begin());
// Calculate the other one
if(m > n)
{
// A = UDV', A'A = VD^2V', AV = UD, ui = A * vi / di
// evecs has already been copied to v, so we can overwrite evecs
for(int i = 0; i < nu; i++)
{
evecs.col(i) /= d[i];
op_orig->perform_op(&evecs(0, i), &u(0, i));
nops++;
}
} else {
// A = UDV', AA' = UD^2U', A'U = VD, vi = A' * ui / di
// evecs has already been copied to u, so we can overwrite evecs
for(int i = 0; i < nv; i++)
{
evecs.col(i) /= d[i];
op_orig->perform_tprod(&evecs(0, i), &v(0, i));
nops++;
}
}
Rcpp::RObject u_ret;
Rcpp::RObject v_ret;
if(nu > 0) u_ret = u; else u_ret = R_NilValue;
if(nv > 0) v_ret = v; else v_ret = R_NilValue;
delete op;
delete op_orig;
return Rcpp::List::create(
Rcpp::Named("d") = d,
Rcpp::Named("u") = u_ret,
Rcpp::Named("v") = v_ret,
Rcpp::Named("niter") = eigs.num_iterations(),
Rcpp::Named("nops") = eigs.num_operations() * 2 + nops
);
END_RCPP
}
TEST(PinholeCamera, functions)
{
const size_t NUM_POINTS = 100;
// instantiate all possible versions of test cameras
std::vector<std::shared_ptr<okvis::cameras::CameraBase> > cameras;
cameras.push_back(
okvis::cameras::PinholeCamera<okvis::cameras::NoDistortion>::createTestObject());
cameras.push_back(
okvis::cameras::PinholeCamera<
okvis::cameras::RadialTangentialDistortion>::createTestObject());
cameras.push_back(
okvis::cameras::PinholeCamera<okvis::cameras::EquidistantDistortion>::createTestObject());
cameras.push_back(
okvis::cameras::PinholeCamera<okvis::cameras::RadialTangentialDistortion8>::createTestObject());
for (size_t c = 0; c < cameras.size(); ++c) {
//std::cout << "Testing " << cameras.at(c)->type() << std::endl;
// try quite a lot of points:
for (size_t i = 0; i < NUM_POINTS; ++i) {
// create a random point in the field of view:
Eigen::Vector2d imagePoint = cameras.at(c)->createRandomImagePoint();
// backProject
Eigen::Vector3d ray;
EXPECT_TRUE(cameras.at(c)->backProject(imagePoint, &ray));
// randomise distance
ray.normalize();
ray *= (0.2 + 8 * (Eigen::Vector2d::Random()[0] + 1.0));
// project
Eigen::Vector2d imagePoint2;
Eigen::Matrix<double, 2, 3> J;
Eigen::Matrix2Xd J_intrinsics;
EXPECT_TRUE(
cameras.at(c)->project(ray, &imagePoint2, &J, &J_intrinsics)
== okvis::cameras::CameraBase::ProjectionStatus::Successful);
// check they are the same
EXPECT_TRUE((imagePoint2 - imagePoint).norm() < 0.01);
// check point Jacobian vs. NumDiff
const double dp = 1.0e-7;
Eigen::Matrix<double, 2, 3> J_numDiff;
for (size_t d = 0; d < 3; ++d) {
Eigen::Vector3d point_p = ray
+ Eigen::Vector3d(d == 0 ? dp : 0, d == 1 ? dp : 0,
d == 2 ? dp : 0);
Eigen::Vector3d point_m = ray
- Eigen::Vector3d(d == 0 ? dp : 0, d == 1 ? dp : 0,
d == 2 ? dp : 0);
Eigen::Vector2d imagePoint_p;
Eigen::Vector2d imagePoint_m;
cameras.at(c)->project(point_p, &imagePoint_p);
cameras.at(c)->project(point_m, &imagePoint_m);
J_numDiff.col(d) = (imagePoint_p - imagePoint_m) / (2 * dp);
}
EXPECT_TRUE((J_numDiff - J).norm() < 0.0001);
// check intrinsics Jacobian
const int numIntrinsics = cameras.at(c)->noIntrinsicsParameters();
Eigen::VectorXd intrinsics;
cameras.at(c)->getIntrinsics(intrinsics);
Eigen::Matrix2Xd J_numDiff_intrinsics;
J_numDiff_intrinsics.resize(2,numIntrinsics);
for (int d = 0; d < numIntrinsics; ++d) {
Eigen::VectorXd di;
di.resize(numIntrinsics);
di.setZero();
di[d] = dp;
Eigen::Vector2d imagePoint_p;
Eigen::Vector2d imagePoint_m;
Eigen::VectorXd intrinsics_p = intrinsics+di;
Eigen::VectorXd intrinsics_m = intrinsics-di;
cameras.at(c)->projectWithExternalParameters(ray, intrinsics_p, &imagePoint_p);
cameras.at(c)->projectWithExternalParameters(ray, intrinsics_m, &imagePoint_m);
J_numDiff_intrinsics.col(d) = (imagePoint_p - imagePoint_m) / (2 * dp);
}
/*std::cout<<J_numDiff_intrinsics<<std::endl;
std::cout<<"----------------"<<std::endl;
std::cout<<J_intrinsics<<std::endl;
std::cout<<"================"<<std::endl;*/
EXPECT_TRUE((J_numDiff_intrinsics - J_intrinsics).norm() < 0.0001);
}
}
}
void KRAveragerCis::add_force_constant_vector(const Eigen::VectorXd & ks, const Eigen::VectorXd & rs) {
for (int i = 0; i < ks.rows(); ++i) {
add_force_constant_tuple(ks(i), rs(i));
}
}
inline vcl_size_t size(Eigen::VectorXd const & v) { return v.rows(); }
int main(int argc, char** argv)
{
ros::init(argc, argv, "parser_test");
ros::NodeHandle nh;
try
{
std::string name = "youbot";
RobotPtr robot = RobotPtr(new Robot(name));
ParserPtr parser = ParserPtr(new Parser());
std::string path = "/home/daichi/Work/catkin_ws/src/ahl_ros_pkg/ahl_robot/ahl_robot/yaml/youbot.yaml";
parser->load(path, robot);
robot->getMobility()->print();
ros::MultiThreadedSpinner spinner;
TfPublisherPtr tf_publisher = TfPublisherPtr(new TfPublisher());
const std::string mnp_name = "mnp";
unsigned long cnt = 0;
const double period = 0.001;
ros::Rate r(1 / period);
ahl_filter::DifferentiatorPtr differentiator = ahl_filter::DifferentiatorPtr(new ahl_filter::PseudoDifferentiator(period, 1.0));
Eigen::VectorXd q = Eigen::VectorXd::Constant(robot->getDOF(mnp_name), 0.0);
Eigen::VectorXd dq = Eigen::VectorXd::Constant(robot->getDOF(mnp_name), 0.0);
differentiator->init(q, dq);
Eigen::VectorXd pre_q = q;
//std::ofstream ofs1;
//ofs1.open("result1.hpp");
//std::ofstream ofs2;
//ofs2.open("result2.hpp");
//std::ofstream ofs3;
//ofs3.open("result3.hpp");
while(ros::ok())
{
q = Eigen::VectorXd::Constant(robot->getDOF(mnp_name), 1.0);
double coeff = 1.0 * sin(2.0 * M_PI * 0.1 * cnt * period);
++cnt;
q = coeff * q;
//q = Eigen::VectorXd::Constant(robot->getDOF(mnp_name), M_PI / 2.0);
q.coeffRef(0) = 0.0;
q.coeffRef(1) = 0.0;
q.coeffRef(2) = 0.0;
robot->update(mnp_name, q);
robot->computeBasicJacobian(mnp_name);
robot->computeMassMatrix(mnp_name);
differentiator->apply(q);
Eigen::VectorXd dq1 = robot->getJointVelocity(mnp_name);
Eigen::VectorXd dq2;
differentiator->copyDerivativeValueTo(dq2);
std::cout << "p : " << q.transpose() << std::endl;
std::cout << "pre_p : " << pre_q.transpose() << std::endl;
Eigen::VectorXd dq3 = (q - pre_q) / period;
pre_q = q;
//std::cout << "dq1 : " << dq1.block(3, 0, dq.rows() - 3, 1).transpose() << std::endl;
//std::cout << "dq2 : " << dq2.block(3, 0, dq.rows() - 3, 1).transpose() << std::endl;
//std::cout << "dq3 : " << dq3.block(3, 0, dq.rows() - 3, 1).transpose() << std::endl;
//std::cout << dq << std::endl << std::endl;
//std::cout << cos(2.0 * M_PI * 0.1 * cnt * 0.1) << std::endl;
tf_publisher->publish(robot, false);
/*
ofs1 << cnt * period << " ";
ofs2 << cnt * period << " ";
ofs3 << cnt * period << " ";
for(unsigned int i = 0; i < dq.rows() - 3; ++i)
{
ofs1 << dq1[i + 3] << " ";
ofs2 << dq2[i + 3] << " ";
ofs3 << dq3[i + 3] << " ";
}
ofs1 << std::endl;
ofs2 << std::endl;
ofs3 << std::endl;
*/
r.sleep();
}
}
catch(ahl_robot::Exception& e)
{
ROS_ERROR_STREAM(e.what());
}
return 0;
}
//==============================================================================
void NullFunction::evalGradient(const Eigen::VectorXd& _x,
Eigen::Map<Eigen::VectorXd> _grad)
{
_grad.resize(_x.size());
_grad.setZero();
}
//==============================================================================
void Problem::setUpperBounds(const Eigen::VectorXd& _ub)
{
assert(static_cast<std::size_t>(_ub.size()) == mDimension && "Invalid size.");
mUpperBounds = _ub;
}
double Statistics::scalarProduct(const Eigen::VectorXd& cv)
{
return cv.dot(cv);
}
//==============================================================================
void Function::evalGradient(const Eigen::VectorXd& _x, Eigen::VectorXd& _grad)
{
Eigen::Map<Eigen::VectorXd> tmpGrad(_grad.data(), _grad.size());
evalGradient(_x, tmpGrad);
}
int main(int argc, char **argv) {
Random::randomize();
std::string paramfile = "multi.ini";
int test = 1;
char c;
while ((c = getopt(argc, argv, "p:h")) != EOF) {
switch (c) {
case 'p':
paramfile = optarg;
break;
case 'h':
default:
std::cerr << "Usage: " << argv[0] << " [options]\n";
std::cerr << "\nOptions:\n";
std::cerr << "-p <file>: use the given parameter file\n";
std::cerr << "-h: this help\n";
return 1;
}
}
TParam p;
p.loadTree(paramfile);
MultiAgentMotionModel *motionModel;
std::string typestr = p("multi_rotor_control/type").toString();
if (typestr == "point2d") {
motionModel = new Point2dMotionModel();
} else if (typestr == "rotor2d") {
motionModel = new Rotor2dMotionModel();
} else {
std::cerr << "Error: Unknown type '" << typestr << "' - exiting\n";
exit(-1);
}
motionModel->init(p);
TargetTrackingController ttc;
ttc.init(p);
std::vector<const SensorModel*> sensorModels = ttc.getTopology().getSensorModels();
if (test == 1) {
// online test
TargetTrajectory tt;
tt.init(p);
EKF ekf(motionModel);
ekf.init(p);
std::ofstream topo_out("topo.out");
std::ofstream multi_out("multi.out");
unsigned int nA = motionModel->getNumAgents();
unsigned int nT = motionModel->getNumTargets();
unsigned int aSD = motionModel->getAgentStateDim();
unsigned int cSD = motionModel->getAgentControlDim();
unsigned int tSD = motionModel->getTargetStateDim();
// test output
Eigen::VectorXd state = p("estimation/initialState").toVectorXd();
for (unsigned int i=0; !tt.atEnd(); ++i) {
Eigen::VectorXd mean = ekf.getMean();
Eigen::MatrixXd cov = ekf.getCovariance();
// Eigen::VectorXd control(nA*cSD);
// Eigen::VectorXd targetPosition = state.segment(aSD*nA, cSD);
// for (unsigned int i=0; i<nA; ++i) {
// control.segment(cSD*i, cSD) = (targetPosition - state.segment(aSD*i, cSD))/p("estimation/motionDt").toDouble();
// }
Eigen::VectorXd control = ttc.getControlTopo(&ekf, motionModel, sensorModels);
multi_out << aSD << " " << cSD << " " << tSD << " 0 0 " << nA << " " << nT << " 0 0 0"
<< " " << state.transpose()
<< " " << control.transpose()
<< " " << mean.transpose();
multi_out << " " << Eigen::Map<Eigen::MatrixXd>(cov.data(), 1, cov.cols()*cov.cols());
// for (int i=0; i<N+1; ++i) {
// multi_out << " " << cov(2*agentStateDim, 2*agentStateDim) << " " << cov(2*agentStateDim, 2*agentStateDim+1) << " " << cov(2*agentStateDim+1, 2*agentStateDim) << " " << cov(2*agentStateDim+1, 2*agentStateDim+1);
// }
multi_out << "\n";
ttc.getTopology().write(topo_out, state);
// simulation
state = motionModel->move(state, control, motionModel->sampleNoise(state, control));
if (Random::uniform() < p("multi_rotor_control/kidnap/target").toDouble()) {
// kidnap target
state.segment(aSD*nA, 2) = tt.randomJump();
ekf.getCovariance().block(aSD*nA, aSD*nA, 4, 4) = Eigen::MatrixXd::Identity(4, 4)*4.0;
} else {
state.segment(aSD*nA, 2) = tt.step();
}
if (Random::uniform() < p("multi_rotor_control/kidnap/agent").toDouble()) {
// kidnap agent
int agent = rand()%nA;
state.segment(aSD*agent, 2) = Eigen::Vector2d(Random::uniform(-1, 1), Random::uniform(-0.5, 0.5));
ekf.getCovariance().block(aSD*agent, aSD*agent, 2, 2) = Eigen::MatrixXd::Identity(2, 2)*4.0;
}
// state estimation
ekf.predict(control);
for (std::vector<const SensorModel*>::const_iterator it = sensorModels.begin();
it != sensorModels.end(); ++it) {
if ((*it)->measurementAvailable(state)) {
Eigen::VectorXd noiseSample = (*it)->sampleNoise(state, (*it)->sense(state));
Eigen::VectorXd measurementSample = (*it)->sense(state, noiseSample);
ekf.correct(measurementSample, *(*it));
}
}
}
}
return 0;
}
void KPLSModel::train()
{
if (descriptor_matrix_.cols() == 0)
{
throw Exception::InconsistentUsage(__FILE__, __LINE__, "Data must be read into the model before training!");
}
// if (descriptor_matrix_.cols() < no_components_)
// {
// throw Exception::TooManyPLSComponents(__FILE__, __LINE__, no_components_, descriptor_matrix_.cols());
// }
kernel->calculateKernelMatrix(descriptor_matrix_, K_);
Eigen::MatrixXd P; // Matrix P saves all vectors p
int cols = K_.cols();
// determine the number of components that are to be created.
// no_components_ contains the number of components desired by the user,
// but obviously we cannot calculate more PLS components than features
int features = descriptor_matrix_.cols();
unsigned int components_to_create = no_components_;
if (features < no_components_) components_to_create = features;
U_.resize(K_.rows(), components_to_create);
loadings_.resize(cols, components_to_create);
weights_.resize(Y_.cols(), components_to_create);
latent_variables_.resize(K_.rows(), components_to_create);
P.resize(cols, components_to_create);
Eigen::VectorXd w;
Eigen::VectorXd t;
Eigen::VectorXd c;
Eigen::VectorXd u = Y_.col(0);
Eigen::VectorXd u_old;
for (unsigned int j = 0; j < components_to_create; j++)
{
for (int i = 0; i < 10000 ; i++)
{
w = K_.transpose()*u / Statistics::scalarProduct(u);
w = w / Statistics::euclNorm(w);
t = K_*w;
c = Y_.transpose()*t / Statistics::scalarProduct(t);
u_old = u;
u = Y_*c / Statistics::scalarProduct(c);
if (Statistics::euclDistance(u, u_old)/Statistics::euclNorm(u) > 0.0000001)
{
continue;
}
else // if u has converged
{
break;
}
}
Eigen::VectorXd p = K_.transpose() * t / Statistics::scalarProduct(t);
K_ -= t * p.transpose();
U_.col(j) = u;
loadings_.col(j) = w;
weights_.col(j) = c;
P.col(j) = p;
latent_variables_.col(j) = t;
}
// try // p's are not orthogonal to each other, so that in rare cases P.t()*loadings_ is not invertible
// {
// loadings_ = loadings_*(P.t()*loadings_).i();
// }
// catch(BALL::Exception::MatrixIsSingular e)
// {
// Eigen::MatrixXd I; I.setToIdentity(P.cols());
// I *= 0.001;
// loadings_ = loadings_*(P.t()*loadings_+I).i();
// }
Eigen::MatrixXd m = P.transpose()*loadings_;
training_result_ = loadings_*m.colPivHouseholderQr().solve(weights_.transpose());
calculateOffsets();
}
std::string vectorString(const Eigen::VectorXd& m)
{
// Make a string from the bytes
return std::string((char *) m.data(), m.size() * sizeof(m(0)));
}
void test_schur_dense_vs_sparse(
const TGraphInitRandom *init_random,
const TGraphInitManual *init_manual,
const double lambda = 1e3 )
{
// A linear system object holds the sparse Jacobians for a set of observations.
my_rba_t::rba_problem_state_t::TLinearSystem lin_system;
size_t nUnknowns_k2k=0, nUnknowns_k2f=0;
if (init_random)
{
randomGenerator.randomize(init_random->random_seed);
nUnknowns_k2k=init_random->nUnknowns_k2k;
nUnknowns_k2f=init_random->nUnknowns_k2f;
}
else
{
nUnknowns_k2k=init_manual->nUnknowns_k2k;
nUnknowns_k2f=init_manual->nUnknowns_k2f;
}
// Fill example Jacobians for the test:
// * 2 keyframes -> 1 k2k edge (edges=unknowns)
// * 6 features with unknown positions.
// * 6*2 observations: each feature seen once from each keyframe
// Note: 6*2*2 = 24 is the minimum >= 1*6+3*6=24 unknowns so
// Hessians are invertible
// -----------------------------------------------------------------
// Create observations:
// Don't populate the symbolic structure, just the numeric part.
static char valid_true = 1; // Just to initialize valid bit pointers to this one.
{
lin_system.dh_dAp.setColCount(nUnknowns_k2k);
lin_system.dh_df.setColCount(nUnknowns_k2f);
size_t idx_obs = 0;
for (size_t nKF=0;nKF<=nUnknowns_k2k;nKF++)
{
my_rba_t::jacobian_traits_t::TSparseBlocksJacobians_dh_dAp::col_t * dh_dAp_i = (nKF==0) ? NULL : (&lin_system.dh_dAp.getCol(nKF-1));
for (size_t nLM=0;nLM<nUnknowns_k2f;nLM++)
{
// Do we have an observation of feature "nLM" from keyframe "nKF"??
bool add_this;
if (init_random)
add_this = (randomGenerator.drawUniform(0.0,1.0)<=init_random->PROB_OBS);
else add_this = init_manual->visible[nKF*nUnknowns_k2f + nLM];
if (add_this)
{
// Create observation: KF[nKF] -> LM[nLM]
my_rba_t::jacobian_traits_t::TSparseBlocksJacobians_dh_df::col_t & dh_df_j = lin_system.dh_df.getCol(nLM);
// Random is ok for this test:
if (dh_dAp_i)
{
randomGenerator.drawGaussian1DMatrix( (*dh_dAp_i)[idx_obs].num );
(*dh_dAp_i)[idx_obs].sym.is_valid = &valid_true;
}
randomGenerator.drawGaussian1DMatrix( dh_df_j[idx_obs].num.setRandom() );
dh_df_j[idx_obs].sym.is_valid = &valid_true;
idx_obs++;
}
}
}
// Debug point:
//if ( idx_obs != (1+nUnknowns_k2k)*nUnknowns_k2f ) cout << "#k2k: " << nUnknowns_k2k << " #k2f: " << nUnknowns_k2f << " #obs: " << idx_obs << " out of max.=" << (1+nUnknowns_k2k)*nUnknowns_k2f << endl;
}
// A default gradient:
Eigen::VectorXd minus_grad; // The negative of the gradient.
const size_t idx_start_f = 6*nUnknowns_k2k;
const size_t nLMs_scalars = 3*nUnknowns_k2f;
const size_t nUnknowns_scalars = idx_start_f + nLMs_scalars;
minus_grad.resize(nUnknowns_scalars);
minus_grad.setOnes();
#if 0
lin_system.dh_dAp.saveToTextFileAsDense("test_dh_dAp.txt");
lin_system.dh_df.saveToTextFileAsDense("test_dh_df.txt");
#endif
// ------------------------------------------------------------
// 1st) Evaluate Schur naively with dense matrices
// ------------------------------------------------------------
CMatrixDouble dense_dh_dAp, dense_dh_df;
lin_system.dh_dAp.getAsDense(dense_dh_dAp);
lin_system.dh_df.getAsDense (dense_dh_df);
const CMatrixDouble dense_HAp = dense_dh_dAp.transpose() * dense_dh_dAp;
const CMatrixDouble dense_Hf = dense_dh_df.transpose() * dense_dh_df;
const CMatrixDouble dense_HApf = dense_dh_dAp.transpose() * dense_dh_df;
CMatrixDouble dense_Hf_plus_lambda = dense_Hf;
for (size_t i=0;i<nLMs_scalars;i++)
dense_Hf_plus_lambda(i,i)+=lambda;
// Schur: naive dense computation:
const CMatrixDouble dense_HAp_schur = dense_HAp - dense_HApf*dense_Hf_plus_lambda.inv()*dense_HApf.transpose();
const Eigen::VectorXd dense_minus_grad_schur = minus_grad.head(idx_start_f) - dense_HApf*(dense_Hf_plus_lambda.inv())*minus_grad.tail(nLMs_scalars);
#if 0
dense_HAp.saveToTextFile("dense_HAp.txt");
dense_Hf.saveToTextFile("dense_Hf.txt");
dense_HApf.saveToTextFile("dense_HApf.txt");
#endif
// ------------------------------------------------------------
// 2nd) Evaluate using sparse Schur implementation
// ------------------------------------------------------------
// Build a list with ALL the unknowns:
vector<my_rba_t::jacobian_traits_t::TSparseBlocksJacobians_dh_dAp::col_t*> dh_dAp;
vector<my_rba_t::jacobian_traits_t::TSparseBlocksJacobians_dh_df::col_t*> dh_df;
for (size_t i=0;i<lin_system.dh_dAp.getColCount();i++)
dh_dAp.push_back( & lin_system.dh_dAp.getCol(i) );
for (size_t i=0;i<lin_system.dh_df.getColCount();i++)
dh_df.push_back( & lin_system.dh_df.getCol(i) );
#if 0
{
CMatrixDouble Jbin;
lin_system.dh_dAp.getBinaryBlocksRepresentation(Jbin);
Jbin.saveToTextFile("test_sparse_jacobs_blocks.txt", mrpt::math::MATRIX_FORMAT_INT );
lin_system.dh_dAp.saveToTextFileAsDense("test_sparse_jacobs.txt");
}
#endif
my_rba_t::hessian_traits_t::TSparseBlocksHessian_Ap HAp;
my_rba_t::hessian_traits_t::TSparseBlocksHessian_f Hf;
my_rba_t::hessian_traits_t::TSparseBlocksHessian_Apf HApf; // This one stores in row-compressed form (i.e. it's stored transposed!!!)
// This resizes and fills in the structs HAp,Hf,HApf from Jacobians:
my_rba_t::sparse_hessian_build_symbolic(
HAp,Hf,HApf,
dh_dAp,dh_df
);
my_rba_t rba;
rba.sparse_hessian_update_numeric(HAp);
rba.sparse_hessian_update_numeric(Hf);
rba.sparse_hessian_update_numeric(HApf);
#if 0
HAp.saveToTextFileAsDense("HAp.txt", true, true );
Hf.saveToTextFileAsDense("Hf.txt", true, true);
HApf.saveToTextFileAsDense("HApf.txt",false, false);
minus_grad.saveToTextFile("minus_grad_Ap.txt");
{ ofstream f("lambda.txt"); f << lambda << endl; }
#endif
// The constructor builds the symbolic Schur.
SchurComplement<
my_rba_t::hessian_traits_t::TSparseBlocksHessian_Ap,
my_rba_t::hessian_traits_t::TSparseBlocksHessian_f,
my_rba_t::hessian_traits_t::TSparseBlocksHessian_Apf
>
schur_compl(
HAp,Hf,HApf, // The different symbolic/numeric Hessian
&minus_grad[0], // minus gradient of the Ap part
&minus_grad[idx_start_f] // minus gradient of the features part
);
schur_compl.numeric_build_reduced_system(lambda);
//cout << "getNumFeaturesFullRank: " << schur_compl.getNumFeaturesFullRank() << endl;
// ------------------------------------------------------------
// 3rd) Both must match!
// ------------------------------------------------------------
// * HAp: Holds the updated Schur complement Hessian.
// * minus_grad: Holds the updated Schur complement -gradient.
CMatrixDouble final_HAp_schur;
HAp.getAsDense(final_HAp_schur, true /* force symmetry, i.e. replicate the half matrix not stored in sparse form */ );
#if 0
final_HAp_schur.saveToTextFile("schur_HAp.txt");
#endif
EXPECT_NEAR( (dense_HAp_schur-final_HAp_schur).array().abs().maxCoeff()/(dense_HAp_schur.array().abs().maxCoeff()),0, 1e-10)
<< "nUnknowns_k2k=" << nUnknowns_k2k << endl
<< "nUnknowns_k2f=" << nUnknowns_k2f << endl
<< "final_HAp_schur:\n" << final_HAp_schur << endl
<< "dense_HAp_schur:\n" << dense_HAp_schur << endl
;
const Eigen::VectorXd final_minus_grad_Ap = minus_grad.head(idx_start_f);
const double Ap_minus_grad_Ap_max_error = (dense_minus_grad_schur-final_minus_grad_Ap).array().abs().maxCoeff();
EXPECT_NEAR( Ap_minus_grad_Ap_max_error/dense_minus_grad_schur.array().abs().maxCoeff(),0, 1e-10)
<< "nUnknowns_k2k=" << nUnknowns_k2k << endl
<< "nUnknowns_k2f=" << nUnknowns_k2f << endl
//<< "dense_minus_grad_schur:\n" << dense_minus_grad_schur
//<< "final_minus_grad_Ap:\n" << final_minus_grad_Ap << endl
;
#if 0
HAp.saveToTextFileAsDense("test_HAp_sparse_schur.txt");
dense_HAp_schur.saveToTextFile("test_HAp_sparse_schur2.txt");
#endif
}
//==============================================================================
void Problem::setOptimalSolution(const Eigen::VectorXd& _optParam)
{
assert(static_cast<std::size_t>(_optParam.size()) == mDimension
&& "Invalid size.");
mOptimalSolution = _optParam;
}
inline void resize(Eigen::VectorXd & v,
std::size_t new_size)
{
v.resize(new_size);
}
inline std::vector<double> eigenVectorToStdVector(const Eigen::VectorXd& v) {
std::vector<double> m(v.size());
for (int i = 0; i < v.size(); i++)
m[i] = v[i];
return m;
}
void multiTaskRecursiveLinearEstimator::setOutputErrorStandardDeviation(const Eigen::VectorXd &sigma_oe_input)
{
assert(sigma_oe.size() == m);
assert(sigma_oe_input.size() == m);
sigma_oe = sigma_oe_input;
}
bool mrpt::vision::pnp::rpnp::compute_pose(
Eigen::Ref<Eigen::Matrix3d> R_, Eigen::Ref<Eigen::Vector3d> t_)
{
// selecting an edge $P_{ i1 }P_{ i2 }$ by n random sampling
int i1 = 0, i2 = 1;
double lmin =
Q(0, i1) * Q(0, i2) + Q(1, i1) * Q(1, i2) + Q(2, i1) * Q(2, i2);
Eigen::MatrixXi rij(n, 2);
R_ = Eigen::MatrixXd::Identity(3, 3);
t_ = Eigen::Vector3d::Zero();
for (int i = 0; i < n; i++)
for (int j = 0; j < 2; j++) rij(i, j) = rand() % n;
for (int ii = 0; ii < n; ii++)
{
int i = rij(ii, 0), j = rij(ii, 1);
if (i == j) continue;
double l = Q(0, i) * Q(0, j) + Q(1, i) * Q(1, j) + Q(2, i) * Q(2, j);
if (l < lmin)
{
i1 = i;
i2 = j;
lmin = l;
}
}
// calculating the rotation matrix of $O_aX_aY_aZ_a$.
Eigen::Vector3d p1, p2, p0, x, y, z, dum_vec;
p1 = P.col(i1);
p2 = P.col(i2);
p0 = (p1 + p2) / 2;
x = p2 - p0;
x /= x.norm();
if (std::abs(x(1)) < std::abs(x(2)))
{
dum_vec << 0, 1, 0;
z = x.cross(dum_vec);
z /= z.norm();
y = z.cross(x);
y /= y.norm();
}
else
{
dum_vec << 0, 0, 1;
y = dum_vec.cross(x);
y /= y.norm();
z = x.cross(y);
x /= x.norm();
}
Eigen::Matrix3d R0;
R0.col(0) = x;
R0.col(1) = y;
R0.col(2) = z;
for (int i = 0; i < n; i++) P.col(i) = R0.transpose() * (P.col(i) - p0);
// Dividing the n - point set into(n - 2) 3 - point subsets
// and setting up the P3P equations
Eigen::Vector3d v1 = Q.col(i1), v2 = Q.col(i2);
double cg1 = v1.dot(v2);
double sg1 = sqrt(1 - cg1 * cg1);
double D1 = (P.col(i1) - P.col(i2)).norm();
Eigen::MatrixXd D4(n - 2, 5);
int j = 0;
Eigen::Vector3d vi;
Eigen::VectorXd rowvec(5);
for (int i = 0; i < n; i++)
{
if (i == i1 || i == i2) continue;
vi = Q.col(i);
double cg2 = v1.dot(vi);
double cg3 = v2.dot(vi);
double sg2 = sqrt(1 - cg2 * cg2);
double D2 = (P.col(i1) - P.col(i)).norm();
double D3 = (P.col(i) - P.col(i2)).norm();
// get the coefficients of the P3P equation from each subset.
rowvec = getp3p(cg1, cg2, cg3, sg1, sg2, D1, D2, D3);
D4.row(j) = rowvec;
j += 1;
if (j > n - 3) break;
}
Eigen::VectorXd D7(8), dumvec(8), dumvec1(5);
D7.setZero();
for (int i = 0; i < n - 2; i++)
{
dumvec1 = D4.row(i);
dumvec = getpoly7(dumvec1);
D7 += dumvec;
}
Eigen::PolynomialSolver<double, 7> psolve(D7.reverse());
Eigen::VectorXcd comp_roots = psolve.roots().transpose();
Eigen::VectorXd real_comp, imag_comp;
real_comp = comp_roots.real();
imag_comp = comp_roots.imag();
Eigen::VectorXd::Index max_index;
double max_real = real_comp.cwiseAbs().maxCoeff(&max_index);
std::vector<double> act_roots_;
int cnt = 0;
for (int i = 0; i < imag_comp.size(); i++)
{
if (std::abs(imag_comp(i)) / max_real < 0.001)
{
act_roots_.push_back(real_comp(i));
cnt++;
}
}
double* ptr = &act_roots_[0];
Eigen::Map<Eigen::VectorXd> act_roots(ptr, cnt);
if (cnt == 0)
{
return false;
}
Eigen::VectorXd act_roots1(cnt);
act_roots1 << act_roots.segment(0, cnt);
std::vector<Eigen::Matrix3d> R_cum(cnt);
std::vector<Eigen::Vector3d> t_cum(cnt);
std::vector<double> err_cum(cnt);
for (int i = 0; i < cnt; i++)
{
double root = act_roots(i);
// Compute the rotation matrix
double d2 = cg1 + root;
Eigen::Vector3d unitx, unity, unitz;
unitx << 1, 0, 0;
unity << 0, 1, 0;
unitz << 0, 0, 1;
x = v2 * d2 - v1;
x /= x.norm();
if (std::abs(unity.dot(x)) < std::abs(unitz.dot(x)))
{
z = x.cross(unity);
z /= z.norm();
y = z.cross(x);
y / y.norm();
}
else
{
y = unitz.cross(x);
y /= y.norm();
z = x.cross(y);
z /= z.norm();
}
R.col(0) = x;
R.col(1) = y;
R.col(2) = z;
// calculating c, s, tx, ty, tz
Eigen::MatrixXd D(2 * n, 6);
D.setZero();
R0 = R.transpose();
Eigen::VectorXd r(
Eigen::Map<Eigen::VectorXd>(R0.data(), R0.cols() * R0.rows()));
for (int j = 0; j < n; j++)
{
double ui = img_pts(j, 0), vi = img_pts(j, 1), xi = P(0, j),
yi = P(1, j), zi = P(2, j);
D.row(2 * j) << -r(1) * yi + ui * (r(7) * yi + r(8) * zi) -
r(2) * zi,
-r(2) * yi + ui * (r(8) * yi - r(7) * zi) + r(1) * zi, -1, 0,
ui, ui * r(6) * xi - r(0) * xi;
D.row(2 * j + 1)
<< -r(4) * yi + vi * (r(7) * yi + r(8) * zi) - r(5) * zi,
-r(5) * yi + vi * (r(8) * yi - r(7) * zi) + r(4) * zi, 0, -1,
vi, vi * r(6) * xi - r(3) * xi;
}
Eigen::MatrixXd DTD = D.transpose() * D;
Eigen::EigenSolver<Eigen::MatrixXd> es(DTD);
Eigen::VectorXd Diag = es.pseudoEigenvalueMatrix().diagonal();
Eigen::MatrixXd V_mat = es.pseudoEigenvectors();
Eigen::MatrixXd::Index min_index;
Diag.minCoeff(&min_index);
Eigen::VectorXd V = V_mat.col(min_index);
V /= V(5);
double c = V(0), s = V(1);
t << V(2), V(3), V(4);
// calculating the camera pose by 3d alignment
Eigen::VectorXd xi, yi, zi;
xi = P.row(0);
yi = P.row(1);
zi = P.row(2);
Eigen::MatrixXd XXcs(3, n), XXc(3, n);
XXc.setZero();
XXcs.row(0) = r(0) * xi + (r(1) * c + r(2) * s) * yi +
(-r(1) * s + r(2) * c) * zi +
t(0) * Eigen::VectorXd::Ones(n);
XXcs.row(1) = r(3) * xi + (r(4) * c + r(5) * s) * yi +
(-r(4) * s + r(5) * c) * zi +
t(1) * Eigen::VectorXd::Ones(n);
XXcs.row(2) = r(6) * xi + (r(7) * c + r(8) * s) * yi +
(-r(7) * s + r(8) * c) * zi +
t(2) * Eigen::VectorXd::Ones(n);
for (int ii = 0; ii < n; ii++)
XXc.col(ii) = Q.col(ii) * XXcs.col(ii).norm();
Eigen::Matrix3d R2;
Eigen::Vector3d t2;
Eigen::MatrixXd XXw = obj_pts.transpose();
calcampose(XXc, XXw, R2, t2);
R_cum[i] = R2;
t_cum[i] = t2;
for (int k = 0; k < n; k++) XXc.col(k) = R2 * XXw.col(k) + t2;
Eigen::MatrixXd xxc(2, n);
xxc.row(0) = XXc.row(0).array() / XXc.row(2).array();
xxc.row(1) = XXc.row(1).array() / XXc.row(2).array();
double res = ((xxc.row(0) - img_pts.col(0).transpose()).norm() +
(xxc.row(1) - img_pts.col(1).transpose()).norm()) /
2;
err_cum[i] = res;
}
int pos_cum =
std::min_element(err_cum.begin(), err_cum.end()) - err_cum.begin();
R_ = R_cum[pos_cum];
t_ = t_cum[pos_cum];
return true;
}
Eigen::VectorXd RBM::operator()(const Eigen::VectorXd& x)
{
v = x.transpose();
sampleHgivenV();
return ph.transpose();
}
//==============================================================================
bool GradientDescentSolver::solve()
{
bool minimized = false;
bool satisfied = false;
std::shared_ptr<Problem> problem = mProperties.mProblem;
if(nullptr == problem)
{
dtwarn << "[GradientDescentSolver::solve] Attempting to solve a nullptr "
<< "problem! We will return false.\n";
return false;
}
double tol = std::abs(mProperties.mTolerance);
double gamma = mGradientP.mStepSize;
size_t dim = problem->getDimension();
if(dim == 0)
{
problem->setOptimalSolution(Eigen::VectorXd());
problem->setOptimumValue(0.0);
return true;
}
Eigen::VectorXd x = problem->getInitialGuess();
assert(x.size() == static_cast<int>(dim));
Eigen::VectorXd lastx = x;
Eigen::VectorXd dx(x.size());
Eigen::VectorXd grad(x.size());
mEqConstraintCostCache.resize(problem->getNumEqConstraints());
mIneqConstraintCostCache.resize(problem->getNumIneqConstraints());
mLastNumIterations = 0;
size_t attemptCount = 0;
do
{
size_t stepCount = 0;
do
{
++mLastNumIterations;
// Perturb the configuration if we have reached an iteration where we are
// supposed to perturb it.
if(mGradientP.mPerturbationStep > 0 && stepCount > 0
&& stepCount%mGradientP.mPerturbationStep == 0)
{
dx = x; // Seed the configuration randomizer with the current configuration
randomizeConfiguration(dx);
// Step the current configuration towards the randomized configuration
// proportionally to a randomized scaling factor
double scale = mGradientP.mMaxPerturbationFactor*mDistribution(mMT);
x += scale*(dx-x);
}
// Check if the equality constraints are satsified
satisfied = true;
for(size_t i=0; i<problem->getNumEqConstraints(); ++i)
{
mEqConstraintCostCache[i] = problem->getEqConstraint(i)->eval(x);
if(std::abs(mEqConstraintCostCache[i]) > tol)
satisfied = false;
}
// Check if the inequality constraints are satisfied
for(size_t i=0; i<problem->getNumIneqConstraints(); ++i)
{
mIneqConstraintCostCache[i] = problem->getIneqConstraint(i)->eval(x);
if(mIneqConstraintCostCache[i] > std::abs(tol))
satisfied = false;
}
Eigen::Map<Eigen::VectorXd> dxMap(dx.data(), dim);
Eigen::Map<Eigen::VectorXd> gradMap(grad.data(), dim);
// Compute the gradient of the objective, combined with the weighted
// gradients of the softened constraints
problem->getObjective()->evalGradient(x, dxMap);
for(int i=0; i < static_cast<int>(problem->getNumEqConstraints()); ++i)
{
if(std::abs(mEqConstraintCostCache[i]) < tol)
continue;
problem->getEqConstraint(i)->evalGradient(x, gradMap);
// Get the user-specified weight if available, otherwise use the default
// weight value
double weight = mGradientP.mEqConstraintWeights.size() > i?
mGradientP.mEqConstraintWeights[i] :
mGradientP.mDefaultConstraintWeight;
// We treat the constraint function as though we are minimizing its
// absolute value. We do not want to treat it as though we are
// minimizing its square, because that could adversely affect the
// curvature of its derivative.
dx += weight * grad * math::sign(mEqConstraintCostCache[i]);
}
for(int i=0; i < static_cast<int>(problem->getNumIneqConstraints()); ++i)
{
if(mIneqConstraintCostCache[i] < tol)
continue;
problem->getIneqConstraint(i)->evalGradient(x, gradMap);
// Get the user-specified weight if available, otherwise use the
// default weight value
double weight = mGradientP.mIneqConstraintWeights.size() > i?
mGradientP.mIneqConstraintWeights[i] :
mGradientP.mDefaultConstraintWeight;
dx += weight * grad;
}
x -= gamma*dx;
clampToBoundary(x);
if((x-lastx).norm() < tol)
minimized = true;
else
minimized = false;
lastx = x;
++stepCount;
if(nullptr != mProperties.mOutStream &&
mProperties.mIterationsPerPrint > 0 &&
stepCount%mProperties.mIterationsPerPrint == 0)
{
*mProperties.mOutStream
<< "[GradientDescentSolver] Progress (attempt #"
<< attemptCount << " | iteration #" << stepCount << ")\n"
<< "cost: " << problem->getObjective()->eval(x) << " | "
<< (minimized? "minimized | " : "not minimized | ")
<< (satisfied? "constraints satisfied | "
: "constraints unsatisfied | ")
<< "x: " << x.transpose() << "\n"
<< "grad: " << dx.transpose() << std::endl;
}
if(stepCount > mProperties.mNumMaxIterations)
break;
} while(!minimized || !satisfied);
if(!minimized || !satisfied)
{
++attemptCount;
if(mGradientP.mMaxAttempts > 0 && attemptCount >= mGradientP.mMaxAttempts)
break;
if(attemptCount-1 < problem->getSeeds().size())
{
x = problem->getSeed(attemptCount-1);
}
else
{
randomizeConfiguration(x);
}
}
} while(!minimized || !satisfied);
mLastConfig = x;
problem->setOptimalSolution(x);
problem->setOptimumValue(problem->getObjective()->eval(x));
return minimized && satisfied;
}
void KRAverager::add_force_constant_vector(const Eigen::VectorXd &force_constants, const Eigen::VectorXd &ranges) {
for (int i = 0; i < force_constants.rows(); ++i) {
add_force_constant_tuple(force_constants(i), ranges(i));
}
}
void SubSystem::setParams(Eigen::VectorXd &xIn)
{
assert(xIn.size() == psize);
for (int i=0; i < psize; i++)
pvals[i] = xIn[i];
}
Eigen::VectorXd readVectorFromFile(const std::string &filename, int_t length)
{
Eigen::VectorXd v = readMatrix(filename);
REQUIRE(v.rows() == length);
return v;
}
inline std::size_t size(Eigen::VectorXd const & v) { return v.rows(); }
void display()
{
using namespace Eigen;
using namespace igl;
using namespace std;
if(!trackball_on && tot_num_samples < 10000)
{
if(S.size() == 0)
{
S.resize(V.rows());
S.setZero();
}
VectorXd Si;
const int num_samples = 20;
ambient_occlusion(ei,V,N,num_samples,Si);
S *= (double)tot_num_samples;
S += Si*(double)num_samples;
tot_num_samples += num_samples;
S /= (double)tot_num_samples;
}
// Convert to 1-intensity
C.conservativeResize(S.rows(),3);
if(ao_on)
{
C<<S,S,S;
C.array() = (1.0-ao_factor*C.array());
}else
{
C.setConstant(1.0);
}
if(ao_normalize)
{
C.col(0) *= ((double)C.rows())/C.col(0).sum();
C.col(1) *= ((double)C.rows())/C.col(1).sum();
C.col(2) *= ((double)C.rows())/C.col(2).sum();
}
C.col(0) *= color(0);
C.col(1) *= color(1);
C.col(2) *= color(2);
glClearColor(back[0],back[1],back[2],0);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// All smooth points
glEnable( GL_POINT_SMOOTH );
glDisable(GL_LIGHTING);
if(lights_on)
{
lights();
}
push_scene();
glEnable(GL_DEPTH_TEST);
glDepthFunc(GL_LEQUAL);
glEnable(GL_NORMALIZE);
glEnable(GL_COLOR_MATERIAL);
glColorMaterial(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE);
push_object();
// Draw the model
// Set material properties
glEnable(GL_COLOR_MATERIAL);
draw_mesh(V,F,N,C);
pop_object();
// Draw a nice floor
glPushMatrix();
const double floor_offset =
-2./bbd*(V.col(1).maxCoeff()-mid(1));
glTranslated(0,floor_offset,0);
const float GREY[4] = {0.5,0.5,0.6,1.0};
const float DARK_GREY[4] = {0.2,0.2,0.3,1.0};
draw_floor(GREY,DARK_GREY);
glPopMatrix();
pop_scene();
report_gl_error();
TwDraw();
glutSwapBuffers();
glutPostRedisplay();
}
void ba81NormalQuad::layer::setStructure(Eigen::ArrayBase<T1> ¶m,
Eigen::MatrixBase<T2> &gmean, Eigen::MatrixBase<T3> &gcov)
{
abilitiesMap.clear();
std::string str = string_snprintf("layer:");
for (int ax=0; ax < gmean.rows(); ++ax) {
if (!abilitiesMask[ax]) continue;
abilitiesMap.push_back(ax);
str += " ";
str += quad->ig.factorNames[ax];
}
str += ":";
itemsMap.clear();
glItemsMap.resize(param.cols(), -1);
for (int ix=0, lx=0; ix < param.cols(); ++ix) {
if (!itemsMask[ix]) continue;
itemsMap.push_back(ix);
glItemsMap[ix] = lx++;
str += string_snprintf(" %d", ix);
}
str += "\n";
//mxLogBig(str);
dataColumns.clear();
dataColumns.reserve(numItems());
totalOutcomes = 0;
for (int ix=0; ix < numItems(); ++ix) {
int outcomes = quad->ig.itemOutcomes[ itemsMap[ix] ];
itemOutcomes.push_back(outcomes);
cumItemOutcomes.push_back(totalOutcomes);
totalOutcomes += outcomes;
dataColumns.push_back(quad->ig.dataColumns[ itemsMap[ix] ]);
}
Eigen::VectorXd mean;
Eigen::MatrixXd cov;
globalToLocalDist(gmean, gcov, mean, cov);
if (mean.size() == 0) {
numSpecific = 0;
primaryDims = 0;
maxDims = 1;
totalQuadPoints = 1;
totalPrimaryPoints = 1;
weightTableSize = 1;
return;
}
numSpecific = 0;
if (quad->ig.twotier) detectTwoTier(param, mean, cov);
if (numSpecific) quad->hasBifactorStructure = true;
primaryDims = cov.cols() - numSpecific;
maxDims = primaryDims + (numSpecific? 1 : 0);
totalQuadPoints = 1;
for (int dx=0; dx < maxDims; dx++) {
totalQuadPoints *= quad->gridSize;
}
totalPrimaryPoints = totalQuadPoints;
weightTableSize = totalQuadPoints;
if (numSpecific) {
totalPrimaryPoints /= quad->gridSize;
weightTableSize *= numSpecific;
}
}
RcppExport SEXP svds_sym(SEXP A_mat_r, SEXP n_scalar_r, SEXP k_scalar_r,
SEXP nu_scalar_r, SEXP nv_scalar_r,
SEXP params_list_r, SEXP mattype_scalar_r)
{
BEGIN_RCPP
Rcpp::List params_svds(params_list_r);
int n = as<int>(n_scalar_r);
int k = as<int>(k_scalar_r);
int nu = as<int>(nu_scalar_r);
int nv = as<int>(nv_scalar_r);
int ncv = as<int>(params_svds["ncv"]);
double tol = as<double>(params_svds["tol"]);
int maxitr = as<int>(params_svds["maxitr"]);
int mattype = as<int>(mattype_scalar_r);
MatProd* op = get_mat_prod_op(A_mat_r, n, n, params_list_r, mattype);
SymEigsSolver<double, LARGEST_MAGN, MatProd> eigs(op, k, ncv);
eigs.init();
int nconv = eigs.compute(maxitr, tol);
if(nconv < k)
Rcpp::warning("only %d singular values converged, less than k = %d", nconv, k);
nu = std::min(nu, nconv);
nv = std::min(nv, nconv);
Eigen::VectorXd evals = eigs.eigenvalues();
Eigen::MatrixXd evecs = eigs.eigenvectors();
Rcpp::NumericVector d(nconv);
Rcpp::NumericMatrix u(n, nu), v(n, nv);
// Copy evals to d
std::copy(evals.data(), evals.data() + nconv, d.begin());
// Copy evecs to u and v with the specified number of columns
std::copy(evecs.data(), evecs.data() + nu * n, u.begin());
std::copy(evecs.data(), evecs.data() + nv * n, v.begin());
// We need to make sure that singular values are nonnegative,
// so move the sign to v.
for(int i = 0; i < nconv; i++)
{
if(d[i] < 0)
{
d[i] = -d[i];
if(i < nv)
{
double* ptr = &v(0, i);
std::transform(ptr, ptr + n, ptr, std::negate<double>());
}
}
}
Rcpp::RObject u_ret;
Rcpp::RObject v_ret;
if(nu > 0) u_ret = u; else u_ret = R_NilValue;
if(nv > 0) v_ret = v; else v_ret = R_NilValue;
delete op;
return Rcpp::List::create(
Rcpp::Named("d") = d,
Rcpp::Named("u") = u_ret,
Rcpp::Named("v") = v_ret,
Rcpp::Named("niter") = eigs.num_iterations(),
Rcpp::Named("nops") = eigs.num_operations()
);
END_RCPP
}
void MagCal::calcMagComp()
{
/*
* Inspired by
* http://davidegironi.blogspot.it/2013/01/magnetometer-calibration-helper-01-for.html#.UriTqkMjulM
*
* Ellipsoid fit from:
* http://www.mathworks.com/matlabcentral/fileexchange/24693-ellipsoid-fit
*
* To use Eigen to convert matlab code, have a look at Eigen/AsciiQuickReference.txt
*/
if (mMagSamples.size() < 9) {
QMessageBox::warning(this, "Magnetometer compensation",
"Too few points.");
return;
}
int samples = mMagSamples.size();
Eigen::VectorXd ex(samples);
Eigen::VectorXd ey(samples);
Eigen::VectorXd ez(samples);
for (int i = 0;i < samples;i++) {
ex(i) = mMagSamples.at(i).at(0);
ey(i) = mMagSamples.at(i).at(1);
ez(i) = mMagSamples.at(i).at(2);
}
Eigen::MatrixXd eD(samples, 9);
for (int i = 0;i < samples;i++) {
eD(i, 0) = ex(i) * ex(i);
eD(i, 1) = ey(i) * ey(i);
eD(i, 2) = ez(i) * ez(i);
eD(i, 3) = 2.0 * ex(i) * ey(i);
eD(i, 4) = 2.0 * ex(i) * ez(i);
eD(i, 5) = 2.0 * ey(i) * ez(i);
eD(i, 6) = 2.0 * ex(i);
eD(i, 7) = 2.0 * ey(i);
eD(i, 8) = 2.0 * ez(i);
}
Eigen::MatrixXd etmp1 = eD.transpose() * eD;
Eigen::MatrixXd etmp2 = eD.transpose() * Eigen::MatrixXd::Ones(samples, 1);
Eigen::VectorXd eV = etmp1.lu().solve(etmp2);
Eigen::MatrixXd eA(4, 4);
eA(0,0)=eV(0); eA(0,1)=eV(3); eA(0,2)=eV(4); eA(0,3)=eV(6);
eA(1,0)=eV(3); eA(1,1)=eV(1); eA(1,2)=eV(5); eA(1,3)=eV(7);
eA(2,0)=eV(4); eA(2,1)=eV(5); eA(2,2)=eV(2); eA(2,3)=eV(8);
eA(3,0)=eV(6); eA(3,1)=eV(7); eA(3,2)=eV(8); eA(3,3)=-1.0;
Eigen::MatrixXd eCenter = -eA.topLeftCorner(3, 3).lu().solve(eV.segment(6, 3));
Eigen::MatrixXd eT = Eigen::MatrixXd::Identity(4, 4);
eT(3, 0) = eCenter(0);
eT(3, 1) = eCenter(1);
eT(3, 2) = eCenter(2);
Eigen::MatrixXd eR = eT * eA * eT.transpose();
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eEv(eR.topLeftCorner(3, 3) * (-1.0 / eR(3, 3)));
Eigen::MatrixXd eVecs = eEv.eigenvectors();
Eigen::MatrixXd eVals = eEv.eigenvalues();
Eigen::MatrixXd eRadii(3, 1);
eRadii(0) = sqrt(1.0 / eVals(0));
eRadii(1) = sqrt(1.0 / eVals(1));
eRadii(2) = sqrt(1.0 / eVals(2));
Eigen::MatrixXd eScale = eRadii.asDiagonal().inverse() * eRadii.minCoeff();
Eigen::MatrixXd eComp = eVecs * eScale * eVecs.transpose();
mMagComp.resize(9);
mMagComp[0] = eComp(0, 0);
mMagComp[1] = eComp(0, 1);
mMagComp[2] = eComp(0, 2);
mMagComp[3] = eComp(1, 0);
mMagComp[4] = eComp(1, 1);
mMagComp[5] = eComp(1, 2);
mMagComp[6] = eComp(2, 0);
mMagComp[7] = eComp(2, 1);
mMagComp[8] = eComp(2, 2);
mMagCompCenter.resize(3);
mMagCompCenter[0] = eCenter(0, 0);
mMagCompCenter[1] = eCenter(1, 0);
mMagCompCenter[2] = eCenter(2, 0);
QVector<double> magX, magY, magZ;
for (int i = 0;i < mMagSamples.size();i++) {
double mx = mMagSamples.at(i).at(0);
double my = mMagSamples.at(i).at(1);
double mz = mMagSamples.at(i).at(2);
mx -= mMagCompCenter.at(0);
my -= mMagCompCenter.at(1);
mz -= mMagCompCenter.at(2);
magX.append(mx * mMagComp.at(0) + my * mMagComp.at(1) + mz * mMagComp.at(2));
magY.append(mx * mMagComp.at(3) + my * mMagComp.at(4) + mz * mMagComp.at(5));
magZ.append(mx * mMagComp.at(6) + my * mMagComp.at(7) + mz * mMagComp.at(8));
}
ui->magSampXyPlot->graph(1)->setData(magX, magY);
ui->magSampXzPlot->graph(1)->setData(magX, magZ);
ui->magSampYzPlot->graph(1)->setData(magY, magZ);
updateMagPlots();
}
