IGL_INLINE void igl::sort_vectors_ccw(
const Eigen::PlainObjectBase<DerivedS>& P,
const Eigen::PlainObjectBase<DerivedS>& N,
Eigen::PlainObjectBase<DerivedI> &order)
{
int half_degree = P.cols()/3;
//local frame
Eigen::Matrix<typename DerivedS::Scalar,1,3> e1 = P.head(3).normalized();
Eigen::Matrix<typename DerivedS::Scalar,1,3> e3 = N.normalized();
Eigen::Matrix<typename DerivedS::Scalar,1,3> e2 = e3.cross(e1);
Eigen::Matrix<typename DerivedS::Scalar,3,3> F; F<<e1.transpose(),e2.transpose(),e3.transpose();
Eigen::Matrix<typename DerivedS::Scalar,Eigen::Dynamic,1> angles(half_degree,1);
for (int i=0; i<half_degree; ++i)
{
Eigen::Matrix<typename DerivedS::Scalar,1,3> Pl = F.colPivHouseholderQr().solve(P.segment(i*3,3).transpose()).transpose();
// assert(fabs(Pl(2))/Pl.cwiseAbs().maxCoeff() <1e-5);
angles[i] = atan2(Pl(1),Pl(0));
}
igl::sort( angles, 1, true, angles, order);
//make sure that the first element is always at the top
while (order[0] != 0)
{
//do a circshift
int temp = order[0];
for (int i =0; i< half_degree-1; ++i)
order[i] = order[i+1];
order(half_degree-1) = temp;
}
}
void lstsq(const Eigen::Matrix<ScalarT, Eigen::Dynamic, Eigen::Dynamic> &A,
const Eigen::Matrix<ScalarT, Eigen::Dynamic, 1> &b,
Eigen::Matrix<ScalarT, Eigen::Dynamic, 1> &x)
{
if (A.rows() == A.cols()) {
// solve via pivoting
x = A.colPivHouseholderQr().solve(b);
} else if (A.rows() > A.cols()) {
// solving via SVD
x = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
} else {
x.fill(std::numeric_limits<ScalarT>::quiet_NaN());
std::cout << "Error solving linear system" << std::endl;
return;
}
}
void ForwardDynamicsABM::calcABMPhase3()
{
const LinkTraverse& traverse = body->linkTraverse();
Link* root = traverse[0];
if(root->jointType == Link::FREE_JOINT){
// - | Ivv trans(Iwv) | * | dvo | = | pf |
// | Iwv Iww | | dw | | ptau |
Eigen::Matrix<double, 6, 6> M;
M << root->Ivv, root->Iwv.transpose(),
root->Iwv, root->Iww;
Eigen::Matrix<double, 6, 1> f;
f << root->pf,
root->ptau;
f *= -1.0;
Eigen::Matrix<double, 6, 1> a(M.colPivHouseholderQr().solve(f));
root->dvo = a.head<3>();
root->dw = a.tail<3>();
} else {
root->dvo.setZero();
root->dw.setZero();
}
int n = traverse.numLinks();
for(int i=1; i < n; ++i){
Link* link = traverse[i];
Link* parent = link->parent;
if(link->jointType != Link::FIXED_JOINT){
link->ddq = (link->uu - (link->hhv.dot(parent->dvo) + link->hhw.dot(parent->dw))) / link->dd;
link->dvo = parent->dvo + link->cv + link->sv * link->ddq;
link->dw = parent->dw + link->cw + link->sw * link->ddq;
}else{
link->ddq = 0.0;
link->dvo = parent->dvo;
link->dw = parent->dw;
}
}
}
void ForwardDynamicsABM::calcABMPhase3()
{
const LinkTraverse& traverse = body->linkTraverse();
DyLink* root = static_cast<DyLink*>(traverse[0]);
if(root->isFreeJoint()){
// - | Ivv trans(Iwv) | * | dvo | = | pf |
// | Iwv Iww | | dw | | ptau |
Eigen::Matrix<double, 6, 6> M;
M << root->Ivv(), root->Iwv().transpose(),
root->Iwv(), root->Iww();
Eigen::Matrix<double, 6, 1> f;
f << root->pf(),
root->ptau();
f *= -1.0;
Eigen::Matrix<double, 6, 1> a(M.colPivHouseholderQr().solve(f));
root->dvo() = a.head<3>();
root->dw() = a.tail<3>();
} else {
root->dvo().setZero();
root->dw().setZero();
}
const int n = traverse.numLinks();
for(int i=1; i < n; ++i){
DyLink* link = static_cast<DyLink*>(traverse[i]);
const DyLink* parent = link->parent();
if(!link->isFixedJoint()){
link->ddq() = (link->uu() - (link->hhv().dot(parent->dvo()) + link->hhw().dot(parent->dw()))) / link->dd();
link->dvo().noalias() = parent->dvo() + link->cv() + link->sv() * link->ddq();
link->dw().noalias() = parent->dw() + link->cw() + link->sw() * link->ddq();
}else{
link->ddq() = 0.0;
link->dvo() = parent->dvo();
link->dw() = parent->dw();
}
}
}
void ColorWheel::setColor(const Color &rgb) {
float r = rgb[0], g = rgb[1], b = rgb[2];
float max = std::max({ r, g, b });
float min = std::min({ r, g, b });
float l = (max + min) / 2;
if (max == min) {
mHue = 0.;
mBlack = 1. - l;
mWhite = l;
} else {
float d = max - min, h;
/* float s = l > 0.5 ? d / (2 - max - min) : d / (max + min); */
if (max == r)
h = (g - b) / d + (g < b ? 6 : 0);
else if (max == g)
h = (b - r) / d + 2;
else
h = (r - g) / d + 4;
h /= 6;
mHue = h;
Eigen::Matrix<float, 4, 3> M;
auto c = hue2rgb(h).rgb().rgb();
M.topLeftCorner<3, 1>() = Eigen::Map<Eigen::Vector3f>(glm::value_ptr(c));
M(3, 0) = 1.;
M.col(1) = Eigen::Vector4f{ 0., 0., 0., 1. };
M.col(2) = Eigen::Vector4f{ 1., 1., 1., 1. };
Eigen::Vector4f rgb4{ rgb[0], rgb[1], rgb[2], 1. };
Eigen::Vector3f bary = M.colPivHouseholderQr().solve(rgb4);
mBlack = bary[1];
mWhite = bary[2];
}
}
inline T log_determinant(const Eigen::Matrix<T, R, C>& m) {
stan::math::check_square("log_determinant", "m", m);
return m.colPivHouseholderQr().logAbsDeterminant();
}
void ProbabilisticStereoTriangulator<CAMERA_GEOMETRY_T>::getUncertainty(
size_t keypointIdxA, size_t keypointIdxB,
const Eigen::Vector4d& homogeneousPoint_A,
Eigen::Matrix3d& outPointUOplus_A, bool& outCanBeInitialized) const {
OKVIS_ASSERT_TRUE_DBG(Exception,frameA_&&frameB_,"initialize with frames before use!");
// also get the point in the other coordinate representation
//Eigen::Vector4d& homogeneousPoint_B=_T_BA*homogeneousPoint_A;
Eigen::Vector4d hPA = homogeneousPoint_A;
// calculate point uncertainty by constructing the lhs of the Gauss-Newton equation system.
// note: the transformation T_WA is assumed constant and identity w.l.o.g.
Eigen::Matrix<double, 9, 9> H = H_;
// keypointA_t& kptA = _frameA_ptr->keypoint(keypointIdxA);
// keypointB_t& kptB = _frameB_ptr->keypoint(keypointIdxB);
Eigen::Vector2d kptA, kptB;
frameA_->getKeypoint(camIdA_, keypointIdxA, kptA);
frameB_->getKeypoint(camIdB_, keypointIdxB, kptB);
// assemble the stuff from the reprojection errors
double keypointStdDev;
frameA_->getKeypointSize(camIdA_, keypointIdxA, keypointStdDev);
keypointStdDev = 0.8 * keypointStdDev / 12.0;
Eigen::Matrix2d inverseMeasurementCovariance = Eigen::Matrix2d::Identity()
* (1.0 / (keypointStdDev * keypointStdDev));
::okvis::ceres::ReprojectionError<CAMERA_GEOMETRY_T> reprojectionErrorA(
frameA_->geometryAs<CAMERA_GEOMETRY_T>(camIdA_), 0, kptA,
inverseMeasurementCovariance);
//typename keypointA_t::measurement_t residualA;
Eigen::Matrix<double, 2, 1> residualA;
Eigen::Matrix<double, 2, 4, Eigen::RowMajor> J_hpA;
Eigen::Matrix<double, 2, 3, Eigen::RowMajor> J_hpA_min;
double* jacobiansA[3];
jacobiansA[0] = 0; // do not calculate, T_WA is fixed identity transform
jacobiansA[1] = J_hpA.data();
jacobiansA[2] = 0; // fixed extrinsics
double* jacobiansA_min[3];
jacobiansA_min[0] = 0; // do not calculate, T_WA is fixed identity transform
jacobiansA_min[1] = J_hpA_min.data();
jacobiansA_min[2] = 0; // fixed extrinsics
const double* parametersA[3];
//const double* test = _poseA.parameters();
parametersA[0] = poseA_.parameters();
parametersA[1] = hPA.data();
parametersA[2] = extrinsics_.parameters();
reprojectionErrorA.EvaluateWithMinimalJacobians(parametersA, residualA.data(),
jacobiansA, jacobiansA_min);
inverseMeasurementCovariance.setIdentity();
frameB_->getKeypointSize(camIdB_, keypointIdxB, keypointStdDev);
keypointStdDev = 0.8 * keypointStdDev / 12.0;
inverseMeasurementCovariance *= 1.0 / (keypointStdDev * keypointStdDev);
::okvis::ceres::ReprojectionError<CAMERA_GEOMETRY_T> reprojectionErrorB(
frameB_->geometryAs<CAMERA_GEOMETRY_T>(camIdB_), 0, kptB,
inverseMeasurementCovariance);
Eigen::Matrix<double, 2, 1> residualB;
Eigen::Matrix<double, 2, 7, Eigen::RowMajor> J_TB;
Eigen::Matrix<double, 2, 6, Eigen::RowMajor> J_TB_min;
Eigen::Matrix<double, 2, 4, Eigen::RowMajor> J_hpB;
Eigen::Matrix<double, 2, 3, Eigen::RowMajor> J_hpB_min;
double* jacobiansB[3];
jacobiansB[0] = J_TB.data();
jacobiansB[1] = J_hpB.data();
jacobiansB[2] = 0; // fixed extrinsics
double* jacobiansB_min[3];
jacobiansB_min[0] = J_TB_min.data();
jacobiansB_min[1] = J_hpB_min.data();
jacobiansB_min[2] = 0; // fixed extrinsics
const double* parametersB[3];
parametersB[0] = poseB_.parameters();
parametersB[1] = hPA.data();
parametersB[2] = extrinsics_.parameters();
reprojectionErrorB.EvaluateWithMinimalJacobians(parametersB, residualB.data(),
jacobiansB, jacobiansB_min);
// evaluate again closer:
hPA.head<3>() = 0.8 * (hPA.head<3>() - T_AB_.r() / 2.0 * hPA[3])
+ T_AB_.r() / 2.0 * hPA[3];
reprojectionErrorB.EvaluateWithMinimalJacobians(parametersB, residualB.data(),
jacobiansB, jacobiansB_min);
if (residualB.transpose() * residualB < 4.0)
outCanBeInitialized = false;
else
outCanBeInitialized = true;
// now add to H:
H.bottomRightCorner<3, 3>() += J_hpA_min.transpose() * J_hpA_min;
H.topLeftCorner<6, 6>() += J_TB_min.transpose() * J_TB_min;
H.topRightCorner<6, 3>() += J_TB_min.transpose() * J_hpB_min;
H.bottomLeftCorner<3, 6>() += J_hpB_min.transpose() * J_TB_min;
H.bottomRightCorner<3, 3>() += J_hpB_min.transpose() * J_hpB_min;
// invert (if invertible) to get covariance:
Eigen::Matrix<double, 9, 9> cov;
if (H.colPivHouseholderQr().rank() < 9) {
outCanBeInitialized = false;
return;
}
cov = H.inverse(); // FIXME: use the QR decomposition for this...
outPointUOplus_A = cov.bottomRightCorner<3, 3>();
}
