void Hamiltonian::set_q(boost::python::list q_) {
/**
Update Hamiltonian coordinates (all are real-valued scalars - the components of the Python list) - Python-friendly
\param[in] q The Python list of real-valued coordinates to be used for Hamiltonian calculations.
Note: this also sets the status_dia and status_adi variables to zero, impliying the Hamiltonian is not
up to date - which is what we want: since the coordinates are changed, the Hamiltonian must be recomputed
From the prafmatic point of view, if you call this function - expect slower performance.
*/
int sz = boost::python::len(q_);
vector<double> tmp_q(sz,0.0);
for(int i=0; i<sz; i++) {
tmp_q[i] = boost::python::extract<double>(q_[i]);
}
set_q(tmp_q);
}
void distribute_forall::reduce1_quantifier(quantifier * q) {
// This transformation is applied after skolemization/quantifier elimination. So, all quantifiers are universal.
SASSERT(q->is_forall());
// This transformation is applied after basic pre-processing steps.
// So, we can assume that
// 1) All (and f1 ... fn) are already encoded as (not (or (not f1 ... fn)))
// 2) All or-formulas are flat (or f1 (or f2 f3)) is encoded as (or f1 f2 f3)
expr * e = get_cached(q->get_expr());
if (m_manager.is_not(e) && m_manager.is_or(to_app(e)->get_arg(0))) {
// found target for simplification
// (forall X (not (or F1 ... Fn)))
// -->
// (and (forall X (not F1))
// ...
// (forall X (not Fn)))
app * or_e = to_app(to_app(e)->get_arg(0));
unsigned num_args = or_e->get_num_args();
expr_ref_buffer new_args(m_manager);
for (unsigned i = 0; i < num_args; i++) {
expr * arg = or_e->get_arg(i);
expr_ref not_arg(m_manager);
// m_bsimp.mk_not applies basic simplifications. For example, if arg is of the form (not a), then it will return a.
m_bsimp.mk_not(arg, not_arg);
quantifier_ref tmp_q(m_manager);
tmp_q = m_manager.update_quantifier(q, not_arg);
expr_ref new_q(m_manager);
elim_unused_vars(m_manager, tmp_q, new_q);
new_args.push_back(new_q);
}
expr_ref result(m_manager);
// m_bsimp.mk_and actually constructs a (not (or ...)) formula,
// it will also apply basic simplifications.
m_bsimp.mk_and(new_args.size(), new_args.c_ptr(), result);
cache_result(q, result);
}
else {
cache_result(q, m_manager.update_quantifier(q, e));
}
}
void PoseGraph::optimize6DoF()
{
while(true)
{
int cur_index = -1;
int first_looped_index = -1;
m_optimize_buf.lock();
while(!optimize_buf.empty())
{
cur_index = optimize_buf.front();
first_looped_index = earliest_loop_index;
optimize_buf.pop();
}
m_optimize_buf.unlock();
if (cur_index != -1)
{
printf("optimize pose graph \n");
TicToc tmp_t;
m_keyframelist.lock();
KeyFrame* cur_kf = getKeyFrame(cur_index);
int max_length = cur_index + 1;
// w^t_i w^q_i
double t_array[max_length][3];
double q_array[max_length][4];
double sequence_array[max_length];
ceres::Problem problem;
ceres::Solver::Options options;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
//ptions.minimizer_progress_to_stdout = true;
//options.max_solver_time_in_seconds = SOLVER_TIME * 3;
options.max_num_iterations = 5;
ceres::Solver::Summary summary;
ceres::LossFunction *loss_function;
loss_function = new ceres::HuberLoss(0.1);
//loss_function = new ceres::CauchyLoss(1.0);
ceres::LocalParameterization* local_parameterization = new ceres::QuaternionParameterization();
list<KeyFrame*>::iterator it;
int i = 0;
for (it = keyframelist.begin(); it != keyframelist.end(); it++)
{
if ((*it)->index < first_looped_index)
continue;
(*it)->local_index = i;
Quaterniond tmp_q;
Matrix3d tmp_r;
Vector3d tmp_t;
(*it)->getVioPose(tmp_t, tmp_r);
tmp_q = tmp_r;
t_array[i][0] = tmp_t(0);
t_array[i][1] = tmp_t(1);
t_array[i][2] = tmp_t(2);
q_array[i][0] = tmp_q.w();
q_array[i][1] = tmp_q.x();
q_array[i][2] = tmp_q.y();
q_array[i][3] = tmp_q.z();
sequence_array[i] = (*it)->sequence;
problem.AddParameterBlock(q_array[i], 4, local_parameterization);
problem.AddParameterBlock(t_array[i], 3);
if ((*it)->index == first_looped_index || (*it)->sequence == 0)
{
problem.SetParameterBlockConstant(q_array[i]);
problem.SetParameterBlockConstant(t_array[i]);
}
//add edge
for (int j = 1; j < 5; j++)
{
if (i - j >= 0 && sequence_array[i] == sequence_array[i-j])
{
Vector3d relative_t(t_array[i][0] - t_array[i-j][0], t_array[i][1] - t_array[i-j][1], t_array[i][2] - t_array[i-j][2]);
Quaterniond q_i_j = Quaterniond(q_array[i-j][0], q_array[i-j][1], q_array[i-j][2], q_array[i-j][3]);
Quaterniond q_i = Quaterniond(q_array[i][0], q_array[i][1], q_array[i][2], q_array[i][3]);
relative_t = q_i_j.inverse() * relative_t;
Quaterniond relative_q = q_i_j.inverse() * q_i;
ceres::CostFunction* vo_function = RelativeRTError::Create(relative_t.x(), relative_t.y(), relative_t.z(),
relative_q.w(), relative_q.x(), relative_q.y(), relative_q.z(),
0.1, 0.01);
problem.AddResidualBlock(vo_function, NULL, q_array[i-j], t_array[i-j], q_array[i], t_array[i]);
}
}
//add loop edge
if((*it)->has_loop)
{
assert((*it)->loop_index >= first_looped_index);
int connected_index = getKeyFrame((*it)->loop_index)->local_index;
Vector3d relative_t;
relative_t = (*it)->getLoopRelativeT();
Quaterniond relative_q;
relative_q = (*it)->getLoopRelativeQ();
ceres::CostFunction* loop_function = RelativeRTError::Create(relative_t.x(), relative_t.y(), relative_t.z(),
relative_q.w(), relative_q.x(), relative_q.y(), relative_q.z(),
0.1, 0.01);
problem.AddResidualBlock(loop_function, loss_function, q_array[connected_index], t_array[connected_index], q_array[i], t_array[i]);
}
if ((*it)->index == cur_index)
break;
i++;
}
m_keyframelist.unlock();
ceres::Solve(options, &problem, &summary);
//std::cout << summary.BriefReport() << "\n";
//printf("pose optimization time: %f \n", tmp_t.toc());
/*
for (int j = 0 ; j < i; j++)
{
printf("optimize i: %d p: %f, %f, %f\n", j, t_array[j][0], t_array[j][1], t_array[j][2] );
}
*/
m_keyframelist.lock();
i = 0;
for (it = keyframelist.begin(); it != keyframelist.end(); it++)
{
if ((*it)->index < first_looped_index)
continue;
Quaterniond tmp_q(q_array[i][0], q_array[i][1], q_array[i][2], q_array[i][3]);
Vector3d tmp_t = Vector3d(t_array[i][0], t_array[i][1], t_array[i][2]);
Matrix3d tmp_r = tmp_q.toRotationMatrix();
(*it)-> updatePose(tmp_t, tmp_r);
if ((*it)->index == cur_index)
break;
i++;
}
Vector3d cur_t, vio_t;
Matrix3d cur_r, vio_r;
cur_kf->getPose(cur_t, cur_r);
cur_kf->getVioPose(vio_t, vio_r);
m_drift.lock();
r_drift = cur_r * vio_r.transpose();
t_drift = cur_t - r_drift * vio_t;
m_drift.unlock();
//cout << "t_drift " << t_drift.transpose() << endl;
//cout << "r_drift " << Utility::R2ypr(r_drift).transpose() << endl;
it++;
for (; it != keyframelist.end(); it++)
{
Vector3d P;
Matrix3d R;
(*it)->getVioPose(P, R);
P = r_drift * P + t_drift;
R = r_drift * R;
(*it)->updatePose(P, R);
}
m_keyframelist.unlock();
updatePath();
}
std::chrono::milliseconds dura(2000);
std::this_thread::sleep_for(dura);
}
return;
}
bool JumpToDocDialogue::run(DocMgr::Connection &db, IDList *id_list)
{
bool retval = false;
clear();
frame();
writestr(2, 0, "Jump to Document");
curs_set(1);
_stack.push_back("");
if (_stack.size() > 10) _stack.erase(_stack.begin());
_sp = _stack.size() - 1;
_id_str = "";
_field = new TextField(_x + 2, _y + 2, _w - 4, _id_str);
_field->focus(true);
_field->display();
doupdate();
InteractorList interaction;
interaction.push_back(*_field);
interaction.push_back(*this);
bool ok = false;
string err_msg;
while (!ok) {
doupdate();
_completed = false;
while (!_completed) { interaction.process_key(); doupdate(); }
ok = true;
if (_id_str != "") {
ok = true;
if (!DocMgr::DocID::valid(_id_str)) {
ok = false;
err_msg = "INVALID DOCUMENT ID!";
} else {
DocMgr::DocID id(_id_str);
if (!id_list->id_visible(id)) {
DocMgr::Query tmp_q(db);
vector<DocMgr::DocID> ids;
tmp_q.run(ids);
bool found = false;
for (int idx = 0; idx < ids.size(); ++idx)
if (ids[idx] == id) { found = true; break; }
if (!found) {
ok = false;
err_msg = "DOCUMENT ID NOT FOUND IN DATABASE!";
} else {
retval = true;
id_list->clear_query();
id_list->set_current(id);
_stack[_sp] = _id_str;
}
} else {
_stack[_sp] = _id_str;
id_list->set_current(id);
if (_sp != _stack.size() - 1) _stack.erase(_stack.end());
}
}
} else {
_stack.erase(_stack.end());
--_sp;
}
if (!ok) {
string blank(getmaxx(stdscr), ' ');
mvaddstr(0, 0, blank.c_str());
wattron(stdscr, A_BOLD);
mvaddstr(0, 0, err_msg.c_str());
wattroff(stdscr, A_BOLD);
wnoutrefresh(stdscr);
_field->display();
}
}
curs_set(0);
clear();
delete _field;
return retval;
}
Types::Transform PointOnPlaneCalibration::estimateTransform(const std::vector<PointPlanePair> & pair_vector)
{
const int size = pair_vector.size();
// Point-Plane Constraints
// Initial system (Eq. 10)
Eigen::MatrixXd system(size, 13);
for (int i = 0; i < size; ++i)
{
const double d = pair_vector[i].plane_.offset();
const Eigen::Vector3d n = pair_vector[i].plane_.normal();
const Types::Point3 & x = pair_vector[i].point_;
system.row(i) << d + n[0] * x[0] + n[1] * x[1] + n[2] * x[2], // q_0^2
2 * n[2] * x[1] - 2 * n[1] * x[2], // q_0 * q_1
2 * n[0] * x[2] - 2 * n[2] * x[0], // q_0 * q_2
2 * n[1] * x[0] - 2 * n[0] * x[1], // q_0 * q_3
d + n[0] * x[0] - n[1] * x[1] - n[2] * x[2], // q_1^2
2 * n[0] * x[1] + 2 * n[1] * x[0], // q_1 * q_2
2 * n[0] * x[2] + 2 * n[2] * x[0], // q_1 * q_3
d - n[0] * x[0] + n[1] * x[1] - n[2] * x[2], // q_2^2
2 * n[1] * x[2] + 2 * n[2] * x[1], // q_2 * q_3
d - n[0] * x[0] - n[1] * x[1] + n[2] * x[2], // q_3^2
n[0], n[1], n[2]; // q'*q*t
}
// Gaussian reduction
for (int k = 0; k < 3; ++k)
for (int i = k + 1; i < size; ++i)
system.row(i) = system.row(i) - system.row(k) * system.row(i)[10 + k] / system.row(k)[10 + k];
// Quaternion q
Eigen::Vector4d q;
// Transform to inhomogeneous (Eq. 13)
bool P_is_ok(false);
while (not P_is_ok)
{
Eigen::Matrix4d P(Eigen::Matrix4d::Random());
while (P.determinant() < 1e-5)
P = Eigen::Matrix4d::Random();
Eigen::MatrixXd reduced_system(size - 3, 10);
for (int i = 3; i < size; ++i)
{
const Eigen::VectorXd & row = system.row(i);
Eigen::Matrix4d Mi_tilde;
Mi_tilde << row[0] , row[1] / 2, row[2] / 2, row[3] / 2,
row[1] / 2, row[4] , row[5] / 2, row[6] / 2,
row[2] / 2, row[5] / 2, row[7] , row[8] / 2,
row[3] / 2, row[6] / 2, row[8] / 2, row[9] ;
Eigen::Matrix4d Mi_bar(P.transpose() * Mi_tilde * P);
reduced_system.row(i - 3) << Mi_bar(0, 0),
Mi_bar(0, 1) + Mi_bar(1, 0),
Mi_bar(0, 2) + Mi_bar(2, 0),
Mi_bar(0, 3) + Mi_bar(3, 0),
Mi_bar(1, 1),
Mi_bar(1, 2) + Mi_bar(2, 1),
Mi_bar(1, 3) + Mi_bar(3, 1),
Mi_bar(2, 2),
Mi_bar(2, 3) + Mi_bar(3, 2),
Mi_bar(3, 3);
}
// Solve A m* = b
Eigen::MatrixXd A = reduced_system.rightCols<9>();
Eigen::VectorXd b = - reduced_system.leftCols<1>();
Eigen::VectorXd m_star = A.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV).solve(b);
Eigen::Vector4d q_bar(1, m_star[0], m_star[1], m_star[2]);
Eigen::VectorXd err(6);
err << q_bar[1] * q_bar[1],
q_bar[1] * q_bar[2],
q_bar[1] * q_bar[3],
q_bar[2] * q_bar[2],
q_bar[2] * q_bar[3],
q_bar[3] * q_bar[3];
err -= m_star.tail<6>();
//if (err.norm() < 0.1) // P is not ok?
P_is_ok = true;
//std::cout << m_star.transpose() << std::endl;
//std::cout << q_bar[1] * q_bar[1] << " " << q_bar[1] * q_bar[2] << " " << q_bar[1] * q_bar[3] << " "
//<< q_bar[2] * q_bar[2] << " " << q_bar[2] * q_bar[3] << " " << q_bar[3] * q_bar[3] << std::endl;
q = P * q_bar;
}
// We want q.w > 0 (Why?)
//if (q[0] < 0)
// q = -q;
Eigen::Vector4d tmp_q(q.normalized());
Eigen::Quaterniond rotation(tmp_q[0], tmp_q[1], tmp_q[2], tmp_q[3]);
Eigen::VectorXd m(10);
m << q[0] * q[0],
q[0] * q[1],
q[0] * q[2],
q[0] * q[3],
q[1] * q[1],
q[1] * q[2],
q[1] * q[3],
q[2] * q[2],
q[2] * q[3],
q[3] * q[3];
// Solve A (q^T q t) = b
Eigen::Matrix3d A = system.topRightCorner<3, 3>();
Eigen::Vector3d b = - system.topLeftCorner<3, 10>() * m;
Eigen::Translation3d translation(A.colPivHouseholderQr().solve(b) / q.squaredNorm());
// Eigen::Quaterniond tmp(pose.linear());
// Eigen::Translation3d tmp2(pose.translation());
// std::cout << "Prev: " << tmp.normalized().coeffs().transpose() << " " << tmp2.vector().transpose() << std::endl;
//std::cout << "PoP : " << rotation.coeffs().transpose() << " " << translation.vector().transpose() << std::endl;
return translation * rotation;
}
