/**
* Updates the state and the state covariance matrix using a laser measurement.
* @param {MeasurementPackage} meas_package
*/
void UKF::UpdateLidar(MeasurementPackage meas_package) {
VectorXd z = meas_package.raw_measurements_;
MatrixXd Zsig = MatrixXd(2, 2 * n_aug_ + 1);
VectorXd z_pred = VectorXd(2);
MatrixXd S = MatrixXd(2, 2);
//transform sigma points into measurement space
for (int i = 0; i < 2 * n_aug_ + 1; i++) {
double px = Xsig_pred_(0, i);
double py = Xsig_pred_(1, i);
Zsig(0, i) = px;
Zsig(1, i) = py;
}
// measurement noise covariance matrix
MatrixXd R = MatrixXd(2, 2);
R << std_laspx_ * std_laspx_, 0,
0, std_laspy_ * std_laspy_;
// Predict radar measurement
PredictMeasurement(2, Zsig, z_pred, S, R);
// Update state
UpdateState(z, z_pred, S, Zsig);
// Calculate NIS
NIS_laser_ = (z - z_pred).transpose() * S.inverse() * (z - z_pred);
}
// [[Rcpp::export]]
NumericVector jzs_sampler(const int iterations, const NumericVector y, const NumericMatrix X, const NumericVector rscale, const IntegerVector gMap, const int incCont, const NumericVector importanceMu, const NumericVector importanceSig, const int progress, const Function callback, const double callbackInterval, const int which)
{
// which = 0 for mc sampler
// which = 1 for importance sampler
int i = 0, j = 0;
const int N = X.nrow();
const int P = X.ncol();
const double ybar = mean(y);
double sumSq = 0, logDetPriorX = 0;
NumericVector logsamples( iterations );
// gMapCounts is not needed for the sampler, but we need to pass something.
NumericVector gMapCounts( max(gMap) + 1 );
NumericMatrix priorX(incCont, incCont);
NumericMatrix CnX(N, P);
NumericMatrix CnytCnX(1, P);
NumericVector Cny(N);
NumericVector XcolMeans(P);
// Compute mean of each column of design matrix
for( i = 0 ; i < P ; i++ )
{
XcolMeans(i) = mean( X( _, i ) );
}
// Compute centered matrices
for( i = 0 ; i < N ; i++ )
{
Cny(i) = y(i) - ybar;
sumSq += Cny(i) * Cny(i);
for( j = 0 ; j < P ; j++ )
{
CnX(i,j) = X(i,j) - XcolMeans(j);
}
}
MatrixXd XtCnX(MatrixXd(P,P).setZero().selfadjointView<Lower>().rankUpdate( MatrixXd((as<Map<MatrixXd> >(CnX))).transpose()));
// Construct prior cov matrix for continuous covariates from X
if(incCont){
for( i = 0 ; i < incCont ; i++ ){
for( j = 0 ; j <= i ; j++ ){
priorX(i,j) = sum( CnX( _ , i) * CnX( _ , j) ) / N;
priorX(j,i) = priorX(i,j);
}
}
logDetPriorX = log_determinant_pos_def(MatrixXd((as<Map<MatrixXd> >(priorX))));
}
// Compute t(Cy) %*% CX
CnytCnX = wrap(MatrixXd((as<Map<MatrixXd> >(Cny))).transpose() * MatrixXd((as<Map<MatrixXd> >(CnX))));
if( which == 0 ){ // mc sampler
jzs_mc_sampler(&logsamples, iterations, sumSq, N, wrap(XtCnX), CnytCnX, rscale, gMap, gMapCounts, priorX, logDetPriorX, incCont, progress, callback, callbackInterval);
}else if( which == 1 ){ // importance sampler
jzs_importance_sampler(&logsamples, iterations, importanceMu, importanceSig, sumSq, N, wrap(XtCnX), CnytCnX, rscale, gMap, gMapCounts, priorX, logDetPriorX, incCont, progress, callback, callbackInterval);
}else{
Rcpp::stop("Invalid sampler specified.");
}
return logsamples;
}
std::pair<bool,Vector3d> Compound::Point::getVector(Manifold* space, int i) {
//std::cout << "Compound.cpp space:\t" << getPosition()->getSpace()->getType() << std::endl;
//std::cout << "Compound.cpp i:\t" << i << std::endl;
//std::cout << "Compound.cpp getPosition()->getSpace():\t" << getPosition()->getSpace() << std::endl;
//std::cout << "Compound.cpp space:\t" << space << std::endl;
if(i <= 0) {
//std::cout << "Compound.cpp i <= 0" << std::endl;
if(getPosition()->getSpace() == space) {
//std::cout << "Compound.cpp getPosition()->getSpace() == space" << std::endl;
return std::make_pair(true,getPosition()->getVector());
} else {
//std::cout << "Compound.cpp getPosition()->getSpace() != space" << std::endl;
return std::make_pair(false,Vector3d());
}
}
//std::cout << "Compound.cpp i > 0" << std::endl;
std::vector<Manifold::PortalPtr>* portals = getPosition()->getSpace()->getPortals();
for(int j=0; j<portals->size(); ++j) {
if(!(*portals)[j]->containsPoint(getPosition().get())) {
std::pair<bool,Vector3d> out = Compound::Point((*portals)[j]->teleport(getPosition())).getVector(space, i-1);
if(out.first) {
return out;
}
}
}
return std::make_pair(false,Vector3d());
}
double signedDistanceInsideConvexHull(
const Ref<const Matrix<double, 2, Dynamic>> &pts,
const Ref<const Vector2d> &q) {
std::vector<Point> hull_pts = convexHull(eigenToPoints(pts));
double d_star = std::numeric_limits<double>::infinity();
Matrix2d R;
R << 0, 1, -1, 0;
for (int i = 0; i < (static_cast<int>(hull_pts.size()) - 1); ++i) {
Vector2d ai = R * Vector2d(hull_pts[i + 1].x - hull_pts[i].x,
hull_pts[i + 1].y - hull_pts[i].y);
double b = ai.transpose() * Vector2d(hull_pts[i].x, hull_pts[i].y);
double n = ai.norm();
if (std::isnormal(n)) {
ai = ai.array() / n;
b = b / n;
double d = b - ai.transpose() * q;
// std::cout << "pt0: " << hull_pts[i].x << " " << hull_pts[i].y <<
// std::endl;
// std::cout << "pt1: " << hull_pts[i+1].x << " " << hull_pts[i+1].y <<
// std::endl;
// std::cout << "ai: " << ai.transpose() << std::endl;
// std::cout << "b: " << b << std::endl;
// std::cout << "d: " << d << std::endl;
if (d < d_star) {
d_star = d;
}
}
}
return d_star;
}
/*
* Constructor.
*/
FusionEKF::FusionEKF() {
is_initialized_ = false;
previous_timestamp_ = 0;
// initializing matrices
R_laser_ = MatrixXd(2, 2);
R_radar_ = MatrixXd(3, 3);
H_laser_ = MatrixXd(2, 4);
Hj_ = MatrixXd(3, 4);
//measurement covariance matrix - laser
R_laser_ << 0.0225, 0,
0, 0.0225;
//measurement covariance matrix - radar
R_radar_ << 0.09, 0, 0,
0, 0.0009, 0,
0, 0, 0.09;
/**
TODO:
* Finish initializing the FusionEKF.
* Set the process and measurement noises
*/
// Initializing P
ekf_.P_ = MatrixXd(4, 4);
ekf_.P_ << 1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1000, 0,
0, 0, 0, 1000;
H_laser_ << 1, 0, 0, 0,
0, 1, 0, 0;
}
static void test_evals()
{
Tensor<float, 2, DataLayout> mat1(2, 3);
Tensor<float, 2, DataLayout> mat2(2, 3);
Tensor<float, 2, DataLayout> mat3(3, 2);
mat1.setRandom();
mat2.setRandom();
mat3.setRandom();
Tensor<float, 2, DataLayout> mat4(3,3);
mat4.setZero();
Eigen::array<DimPair, 1> dims3 = {{DimPair(0, 0)}};
typedef TensorEvaluator<decltype(mat1.contract(mat2, dims3)), DefaultDevice> Evaluator;
Evaluator eval(mat1.contract(mat2, dims3), DefaultDevice());
eval.evalTo(mat4.data());
EIGEN_STATIC_ASSERT(Evaluator::NumDims==2ul, YOU_MADE_A_PROGRAMMING_MISTAKE);
VERIFY_IS_EQUAL(eval.dimensions()[0], 3);
VERIFY_IS_EQUAL(eval.dimensions()[1], 3);
VERIFY_IS_APPROX(mat4(0,0), mat1(0,0)*mat2(0,0) + mat1(1,0)*mat2(1,0));
VERIFY_IS_APPROX(mat4(0,1), mat1(0,0)*mat2(0,1) + mat1(1,0)*mat2(1,1));
VERIFY_IS_APPROX(mat4(0,2), mat1(0,0)*mat2(0,2) + mat1(1,0)*mat2(1,2));
VERIFY_IS_APPROX(mat4(1,0), mat1(0,1)*mat2(0,0) + mat1(1,1)*mat2(1,0));
VERIFY_IS_APPROX(mat4(1,1), mat1(0,1)*mat2(0,1) + mat1(1,1)*mat2(1,1));
VERIFY_IS_APPROX(mat4(1,2), mat1(0,1)*mat2(0,2) + mat1(1,1)*mat2(1,2));
VERIFY_IS_APPROX(mat4(2,0), mat1(0,2)*mat2(0,0) + mat1(1,2)*mat2(1,0));
VERIFY_IS_APPROX(mat4(2,1), mat1(0,2)*mat2(0,1) + mat1(1,2)*mat2(1,1));
VERIFY_IS_APPROX(mat4(2,2), mat1(0,2)*mat2(0,2) + mat1(1,2)*mat2(1,2));
Tensor<float, 2, DataLayout> mat5(2,2);
mat5.setZero();
Eigen::array<DimPair, 1> dims4 = {{DimPair(1, 1)}};
typedef TensorEvaluator<decltype(mat1.contract(mat2, dims4)), DefaultDevice> Evaluator2;
Evaluator2 eval2(mat1.contract(mat2, dims4), DefaultDevice());
eval2.evalTo(mat5.data());
EIGEN_STATIC_ASSERT(Evaluator2::NumDims==2ul, YOU_MADE_A_PROGRAMMING_MISTAKE);
VERIFY_IS_EQUAL(eval2.dimensions()[0], 2);
VERIFY_IS_EQUAL(eval2.dimensions()[1], 2);
VERIFY_IS_APPROX(mat5(0,0), mat1(0,0)*mat2(0,0) + mat1(0,1)*mat2(0,1) + mat1(0,2)*mat2(0,2));
VERIFY_IS_APPROX(mat5(0,1), mat1(0,0)*mat2(1,0) + mat1(0,1)*mat2(1,1) + mat1(0,2)*mat2(1,2));
VERIFY_IS_APPROX(mat5(1,0), mat1(1,0)*mat2(0,0) + mat1(1,1)*mat2(0,1) + mat1(1,2)*mat2(0,2));
VERIFY_IS_APPROX(mat5(1,1), mat1(1,0)*mat2(1,0) + mat1(1,1)*mat2(1,1) + mat1(1,2)*mat2(1,2));
Tensor<float, 2, DataLayout> mat6(2,2);
mat6.setZero();
Eigen::array<DimPair, 1> dims6 = {{DimPair(1, 0)}};
typedef TensorEvaluator<decltype(mat1.contract(mat3, dims6)), DefaultDevice> Evaluator3;
Evaluator3 eval3(mat1.contract(mat3, dims6), DefaultDevice());
eval3.evalTo(mat6.data());
EIGEN_STATIC_ASSERT(Evaluator3::NumDims==2ul, YOU_MADE_A_PROGRAMMING_MISTAKE);
VERIFY_IS_EQUAL(eval3.dimensions()[0], 2);
VERIFY_IS_EQUAL(eval3.dimensions()[1], 2);
VERIFY_IS_APPROX(mat6(0,0), mat1(0,0)*mat3(0,0) + mat1(0,1)*mat3(1,0) + mat1(0,2)*mat3(2,0));
VERIFY_IS_APPROX(mat6(0,1), mat1(0,0)*mat3(0,1) + mat1(0,1)*mat3(1,1) + mat1(0,2)*mat3(2,1));
VERIFY_IS_APPROX(mat6(1,0), mat1(1,0)*mat3(0,0) + mat1(1,1)*mat3(1,0) + mat1(1,2)*mat3(2,0));
VERIFY_IS_APPROX(mat6(1,1), mat1(1,0)*mat3(0,1) + mat1(1,1)*mat3(1,1) + mat1(1,2)*mat3(2,1));
}
/**
* Updates the state and the state covariance matrix using a radar measurement.
* @param {MeasurementPackage} meas_package
*/
void UKF::UpdateRadar(MeasurementPackage meas_package) {
VectorXd z = meas_package.raw_measurements_;
MatrixXd Zsig = MatrixXd(3, 2 * n_aug_ + 1);
VectorXd z_pred = VectorXd(3);
MatrixXd S = MatrixXd(3, 3);
//transform sigma points into radar measurement space
for (int i = 0; i < 2 * n_aug_ + 1; i++) {
double px = Xsig_pred_(0, i);
double py = Xsig_pred_(1, i);
double v = Xsig_pred_(2, i);
double yaw = Xsig_pred_(3, i);
double vx = cos(yaw) * v;
double vy = sin(yaw) * v;
Zsig(0, i) = sqrt(px * px + py * py); //rho
Zsig(1, i) = atan2(py, px); //phi
Zsig(2, i) = (px * vx + py * vy ) / sqrt(px * px + py * py); //rho_dot
}
//measurement noise covariance matrix
MatrixXd R = MatrixXd(3, 3);
R << std_radr_ * std_radr_, 0, 0,
0, std_radphi_ * std_radphi_, 0,
0, 0, std_radrd_ * std_radrd_;
// Predict radar measurement with given Sigma predictions
PredictMeasurement(3, Zsig, z_pred, S, R);
// Update the state
UpdateState(z, z_pred, S, Zsig);
// Calculate NIS
NIS_radar_ = (z - z_pred).transpose() * S.inverse() * (z - z_pred);
}
RcppExport SEXP tcrossprodvec (SEXP X)
{
using namespace Rcpp;
using namespace RcppEigen;
try {
using Eigen::Map;
using Eigen::MatrixXd;
using Eigen::Lower;
typedef float Scalar;
typedef double Double;
typedef Eigen::Matrix<Double, Eigen::Dynamic, Eigen::Dynamic> Matrix;
typedef Eigen::Matrix<Double, Eigen::Dynamic, 1> Vector;
typedef Eigen::Map<const Matrix> MapMat;
typedef Eigen::Map<const Vector> MapVec;
const Eigen::Map<VectorXd> A(as<Map<VectorXd> >(X));
//MapMat A(as<MapMat>(X));
const int n(A.rows());
MatrixXd AtA(MatrixXd(n, n).setZero());
AtA.bottomRightCorner(n, n) = MatrixXd(n, n).setZero().selfadjointView<Lower>().rankUpdate(A);
return wrap(AtA);
} catch (std::exception &ex) {
forward_exception_to_r(ex);
} catch (...) {
::Rf_error("C++ exception (unknown reason)");
}
return R_NilValue; //-Wall
}
// We hard-code many of the parameters.
static std::tuple<EllipseGeometry, Eigen::MatrixX2f> generate_problem(size_t n, float sigma)
{
using Eigen::Vector2f;
auto g = get_ellipse_generator(Vector2f(MAX_SIZE/4, MAX_SIZE-MAX_SIZE/4), Vector2f(50, MAX_RADIUS), 0.1, 2*M_PI/32, sigma);
Eigen::MatrixX2f ret(n, 2);
for (size_t i = 0; i < n; ++i)
ret.row(i) = g();
return std::make_tuple(g.geometry(), ret);
}
Vector3d Compound::PointOfReference::vectorFromPointAndNearVector(Compound::PointPtr point, Vector3d vector, int i) {
if(i > 10 | vector.squaredNorm() > 10000 | vector != vector) {
//std::cout << "Compound.cpp vector:\n" << vector << std::endl;
//return vector;
return Vector3d(0,0,0);
}
Vector3d v1 = point->getPosition()->getVector();
//Vector3d v0 = point->getPosition()->getVector();
//assert(v0 == v0);
double epsilon = 0.00001;
Manifold* space = point->getPosition()->getSpace();
//std::cout << "Compound.cpp i:\t" << i << std::endl;
Vector3d v0 = this->pointFromVector(vector)->getVector(space);
Vector3d vx = this->pointFromVector(vector + Vector3d(epsilon,0,0))->getVector(space);
Vector3d vy = this->pointFromVector(vector + Vector3d(0,epsilon,0))->getVector(space);
Vector3d vz = this->pointFromVector(vector + Vector3d(0,0,epsilon))->getVector(space);
assert(vz == vz);
Matrix3d jacobean;
jacobean << vx-v0,vy-v0,vz-v0;
jacobean /= epsilon;
/*std::cout << "getPosition()->getVector():\n" << getPosition()->getVector() << std::endl;
std::cout << "vector:\n" << vector << std::endl;
std::cout << "v0:\n" << v0 << std::endl;
std::cout << "vx:\n" << vx << std::endl;
std::cout << "vy:\n" << vy << std::endl;
std::cout << "vz:\n" << vz << std::endl;*/
//std::cout << "jacobean:\n" << jacobean << std::endl;
Vector3d delta = jacobean.inverse()*(v1-v0);
//std::cout << "v1-v0:\n" << v1-v0 << std::endl;
//std::cout << "delta:\n" << delta << std::endl;
if(delta != delta) {
return Vector3d(0,0,0);
}
//std::cout << "delta.norm():\t" << delta.norm() << std::endl;
double squaredNorm = delta.squaredNorm();
double max = 5;
if(squaredNorm > max*max) {
delta = max*delta/sqrt(squaredNorm);
}
if(squaredNorm < EPSILON*EPSILON) {
/*std::cout << "v1-v0:\n" << v1-v0 << std::endl;
std::cout << "delta:\n" << delta << std::endl;
std::cout << "vector:\n" << vector << std::endl;
std::cout << "delta+vector:\n" << delta+vector << std::endl;
std::cout << "pointOfReference->vectorFromPoint(point):\n" << pointOfReference->getGeodesic(point->getPosition())->getVector() << std::endl;*/
assert(pointFromVector(delta+vector)->getPosition()->getSpace() == point->getPosition()->getSpace());
assert((pointFromVector(delta+vector)->getPosition()->getVector() - point->getPosition()->getVector()).squaredNorm() < EPSILON);
/*assert((pointOfReference->getSpace()->pointFromVector(pointOfReference, vector)->getVector() - point->getPosition()->getVector()).squaredNorm() < EPSILON);
assert((this->pointFromVector(delta+vector)->getPosition()->getVector() - pointOfReference->getSpace()->pointFromVector(pointOfReference, vector)->getVector()).squaredNorm() < EPSILON);
assert((delta+vector - pointOfReference->getSpace()->vectorFromPoint(pointOfReference, point->getPosition())).squaredNorm() < EPSILON);*/ //Only for portals that connect to themselves.
//std::cout << "i:\t" << i << std::endl;
return delta+vector;
} else {
return vectorFromPointAndNearVector(point, delta+vector, i+1);
}
}
static void test_multidims()
{
Tensor<float, 3, DataLayout> mat1(2, 2, 2);
Tensor<float, 4, DataLayout> mat2(2, 2, 2, 2);
mat1.setRandom();
mat2.setRandom();
Tensor<float, 3, DataLayout> mat3(2, 2, 2);
mat3.setZero();
Eigen::array<DimPair, 2> dims = {{DimPair(1, 2), DimPair(2, 3)}};
typedef TensorEvaluator<decltype(mat1.contract(mat2, dims)), DefaultDevice> Evaluator;
Evaluator eval(mat1.contract(mat2, dims), DefaultDevice());
eval.evalTo(mat3.data());
EIGEN_STATIC_ASSERT(Evaluator::NumDims==3ul, YOU_MADE_A_PROGRAMMING_MISTAKE);
VERIFY_IS_EQUAL(eval.dimensions()[0], 2);
VERIFY_IS_EQUAL(eval.dimensions()[1], 2);
VERIFY_IS_EQUAL(eval.dimensions()[2], 2);
VERIFY_IS_APPROX(mat3(0,0,0), mat1(0,0,0)*mat2(0,0,0,0) + mat1(0,1,0)*mat2(0,0,1,0) +
mat1(0,0,1)*mat2(0,0,0,1) + mat1(0,1,1)*mat2(0,0,1,1));
VERIFY_IS_APPROX(mat3(0,0,1), mat1(0,0,0)*mat2(0,1,0,0) + mat1(0,1,0)*mat2(0,1,1,0) +
mat1(0,0,1)*mat2(0,1,0,1) + mat1(0,1,1)*mat2(0,1,1,1));
VERIFY_IS_APPROX(mat3(0,1,0), mat1(0,0,0)*mat2(1,0,0,0) + mat1(0,1,0)*mat2(1,0,1,0) +
mat1(0,0,1)*mat2(1,0,0,1) + mat1(0,1,1)*mat2(1,0,1,1));
VERIFY_IS_APPROX(mat3(0,1,1), mat1(0,0,0)*mat2(1,1,0,0) + mat1(0,1,0)*mat2(1,1,1,0) +
mat1(0,0,1)*mat2(1,1,0,1) + mat1(0,1,1)*mat2(1,1,1,1));
VERIFY_IS_APPROX(mat3(1,0,0), mat1(1,0,0)*mat2(0,0,0,0) + mat1(1,1,0)*mat2(0,0,1,0) +
mat1(1,0,1)*mat2(0,0,0,1) + mat1(1,1,1)*mat2(0,0,1,1));
VERIFY_IS_APPROX(mat3(1,0,1), mat1(1,0,0)*mat2(0,1,0,0) + mat1(1,1,0)*mat2(0,1,1,0) +
mat1(1,0,1)*mat2(0,1,0,1) + mat1(1,1,1)*mat2(0,1,1,1));
VERIFY_IS_APPROX(mat3(1,1,0), mat1(1,0,0)*mat2(1,0,0,0) + mat1(1,1,0)*mat2(1,0,1,0) +
mat1(1,0,1)*mat2(1,0,0,1) + mat1(1,1,1)*mat2(1,0,1,1));
VERIFY_IS_APPROX(mat3(1,1,1), mat1(1,0,0)*mat2(1,1,0,0) + mat1(1,1,0)*mat2(1,1,1,0) +
mat1(1,0,1)*mat2(1,1,0,1) + mat1(1,1,1)*mat2(1,1,1,1));
Tensor<float, 2, DataLayout> mat4(2, 2);
Tensor<float, 3, DataLayout> mat5(2, 2, 2);
mat4.setRandom();
mat5.setRandom();
Tensor<float, 1, DataLayout> mat6(2);
mat6.setZero();
Eigen::array<DimPair, 2> dims2({{DimPair(0, 1), DimPair(1, 0)}});
typedef TensorEvaluator<decltype(mat4.contract(mat5, dims2)), DefaultDevice> Evaluator2;
Evaluator2 eval2(mat4.contract(mat5, dims2), DefaultDevice());
eval2.evalTo(mat6.data());
EIGEN_STATIC_ASSERT(Evaluator2::NumDims==1ul, YOU_MADE_A_PROGRAMMING_MISTAKE);
VERIFY_IS_EQUAL(eval2.dimensions()[0], 2);
VERIFY_IS_APPROX(mat6(0), mat4(0,0)*mat5(0,0,0) + mat4(1,0)*mat5(0,1,0) +
mat4(0,1)*mat5(1,0,0) + mat4(1,1)*mat5(1,1,0));
VERIFY_IS_APPROX(mat6(1), mat4(0,0)*mat5(0,0,1) + mat4(1,0)*mat5(0,1,1) +
mat4(0,1)*mat5(1,0,1) + mat4(1,1)*mat5(1,1,1));
}
void
GLWindow::init()
{
/* Use depth buffering for hidden surface elimination. */
glEnable(GL_DEPTH_TEST);
camera_.setFovY(3.14f / 4);
camera_.setPosition(Vector3f(0, 0, -5));
camera_.setTarget(Vector3f(0, 0, 0));
setView();
glewInit();
if (GLEW_ARB_vertex_shader && GLEW_ARB_fragment_shader)
std::cout << ("Ready for GLSL\n");
}
Camera::Camera() {
//mtx_ = new std::mutex();
fov_current_ = 60.0f;
fov_future_ = 60.0f;
aspect_ = 1.0f;
depth_near_ = 1.0f;
depth_far_ = 1000.f;
roll_correction_ = true;
always_update_ = false;
rotation_current_ = AngleAxis<float>(0, Vector3f(1.0f, 0.0f, 0.0f));
rotation_future_ = AngleAxis<float>(0, Vector3f(1.0f, 0.0f, 0.0f));
translation_current_ = Vector3f(0, 0, 0);
translation_future_ = Vector3f(0, 0, 0);
translation_speed_ = 1;
projection_dirty_ = true;
timer_ = new QTimer();
timer_->connect(timer_, &QTimer::timeout, [&] () {
Eigen::Vector3f trans_diff = translation_future_ - translation_current_;
bool good_enough_rotation = rotation_current_.isApprox(rotation_future_);
float fov_diff = fov_future_ - fov_current_;
if(trans_diff.norm() > 1e-4 || !good_enough_rotation || fabs(fov_diff) > 1e-10) {
translation_current_ = translation_current_ + trans_diff * 0.5;
rotation_current_ = rotation_current_.slerp(0.5, rotation_future_);
fov_current_ = fov_future_ * 0.5 + fov_current_ * 0.5;
if(fabs(fov_diff) > 1e-10)
projection_dirty_ = true;
emit updated();
}
else {
if(always_update_)
emit updated();
translation_speed_ = 1;
}
});
timer_->start(1000/60);
}
Transformation<double> StereoCartography::estimateOdometry(const vector<Feature> & featureVec)
{
//Matching
int numLandmarks = LM.size();
int numActive = min(300, numLandmarks);
vector<Feature> lmFeatureVec;
// cout << "ca va" << endl;
for (unsigned int i = numLandmarks - numActive; i < numLandmarks; i++)
{
lmFeatureVec.push_back(Feature(Vector2d(0, 0), LM[i].d));
}
// cout << "ca va" << endl;
Matcher matcher;
vector<int> matchVec;
matcher.bruteForce(featureVec, lmFeatureVec, matchVec);
Odometry odometry(trajectory.back(), stereo.TbaseCam1, stereo.cam1);
// cout << "ca va" << endl;
for (unsigned int i = 0; i < featureVec.size(); i++)
{
const int match = matchVec[i];
if (match == -1) continue;
odometry.observationVec.push_back(featureVec[i].pt);
odometry.cloud.push_back(LM[numLandmarks - numActive + match].X);
}
// cout << "cloud : " << odometry.cloud.size() << endl;
//RANSAC
odometry.Ransac();
// cout << odometry.TorigBase << endl;
//Final transformation computation
odometry.computeTransformation();
// cout << odometry.TorigBase << endl;
return odometry.TorigBase;
}
void KalmanFilter::UpdateEKF(const VectorXd &z) {
/**
TODO:
* update the state by using Extended Kalman Filter equations
*/
float rho = sqrt(x_(0)*x_(0) + x_(1)*x_(1));
float phi = atan2(x_(1), x_(0));
float rho_dot = (x_(0)*x_(2) + x_(1)*x_(3))/rho;
VectorXd h = VectorXd(3);
h << rho,phi,rho_dot;
VectorXd y = z - h;
y[1] = atan2(sin(y[1]),cos(y[1]));
MatrixXd Ht = H_.transpose();
MatrixXd S = H_ * P_ * Ht + R_;
MatrixXd Si = S.inverse();
MatrixXd PHt = P_ * Ht;
MatrixXd K = PHt * Si;
//new estimate
x_ = x_ + (K * y);
long x_size = x_.size();
MatrixXd I = MatrixXd::Identity(x_size, x_size);
P_ = (I - K * H_) * P_;
}
void UKF::UpdateState(const VectorXd &z, const VectorXd &z_pred, const MatrixXd &S, const MatrixXd &Zsig) {
int n_z = z_pred.rows();
// calculate cross correlation
MatrixXd Tc = MatrixXd(n_x_, n_z);
Tc.fill(0.0);
for (int i = 0; i < 2 * n_aug_ + 1; i++) {
VectorXd z_diff = Zsig.col(i) - z_pred;
while (z_diff(1) > M_PI) z_diff(1) -= 2. * M_PI;
while (z_diff(1) < -M_PI) z_diff(1) += 2. * M_PI;
VectorXd x_diff = Xsig_pred_.col(i) - x_;
while (x_diff(3) > M_PI) x_diff(3) -= 2. * M_PI;
while (x_diff(3) < -M_PI) x_diff(3) += 2. * M_PI;
Tc = Tc + weights_(i) * x_diff * z_diff.transpose();
}
MatrixXd K = Tc * S.inverse(); // Kalman gain K
VectorXd z_diff = z - z_pred;
while (z_diff(1) > M_PI) z_diff(1) -= 4. * M_PI;
while (z_diff(1) < -M_PI) z_diff(1) += 4. * M_PI;
//update
x_ = x_ + K * z_diff;
P_ = P_ - K * S * K.transpose();
}
GTEST_TEST(testIK, atlasIK) {
RigidBodyTree model(
GetDrakePath() + "/examples/Atlas/urdf/atlas_minimal_contact.urdf");
Vector2d tspan;
tspan << 0, 1;
VectorXd q0 = model.getZeroConfiguration();
// The state frame of cpp model does not match with the state frame of MATLAB
// model, since the dofname_to_dofnum is different in cpp and MATLAB
q0(2) = 0.8;
Vector3d com_lb = Vector3d::Zero();
Vector3d com_ub = Vector3d::Zero();
com_lb(2) = 0.9;
com_ub(2) = 1.0;
WorldCoMConstraint com_kc(&model, com_lb, com_ub, tspan);
std::vector<RigidBodyConstraint*> constraint_array;
constraint_array.push_back(&com_kc);
IKoptions ikoptions(&model);
VectorXd q_sol(model.number_of_positions());
q_sol.setZero();
int info = 0;
std::vector<std::string> infeasible_constraint;
inverseKin(&model, q0, q0, constraint_array.size(), constraint_array.data(),
ikoptions, &q_sol, &info, &infeasible_constraint);
printf("info = %d\n", info);
EXPECT_EQ(info, 1);
KinematicsCache<double> cache = model.doKinematics(q_sol);
Vector3d com = model.centerOfMass(cache);
printf("%5.6f\n%5.6f\n%5.6f\n", com(0), com(1), com(2));
EXPECT_TRUE(
CompareMatrices(com, Vector3d(0, 0, 1), 1e-6,
MatrixCompareType::absolute));
}
//port faster cross product
RcppExport SEXP listtest(SEXP len, SEXP sizea, SEXP sizeb)
{
using namespace Rcpp;
using namespace RcppEigen;
try {
using Eigen::Map;
using Eigen::MatrixXd;
using Eigen::Lower;
typedef float Scalar;
typedef double Double;
typedef Eigen::Matrix<Double, Eigen::Dynamic, Eigen::Dynamic> Matrix;
typedef Eigen::Matrix<Double, Eigen::Dynamic, 1> Vector;
typedef Eigen::Map<const Matrix> MapMat;
typedef Eigen::Map<const Vector> MapVec;
const int length(as<int>(len));
const int sizeA(as<int>(sizea));
const int sizeB(as<int>(sizeb));
//MapMat A(as<MapMat>(X));
std::vector<Eigen::MatrixXd> retlist(length);
for (int i = 0; i < length; ++i)
{
retlist[i] = MatrixXd(sizeA, sizeB);
}
return wrap(retlist);
} catch (std::exception &ex) {
forward_exception_to_r(ex);
} catch (...) {
::Rf_error("C++ exception (unknown reason)");
}
return R_NilValue; //-Wall
}
pcl::gpu::KinfuTracker::KinfuTracker (int rows, int cols) : rows_(rows), cols_(cols), global_time_(0), max_icp_distance_(0), integration_metric_threshold_(0.f), disable_icp_(false)
{
const Vector3f volume_size = Vector3f::Constant (VOLUME_SIZE);
const Vector3i volume_resolution(VOLUME_X, VOLUME_Y, VOLUME_Z);
tsdf_volume_ = TsdfVolume::Ptr( new TsdfVolume(volume_resolution) );
tsdf_volume_->setSize(volume_size);
setDepthIntrinsics (KINFU_DEFAULT_DEPTH_FOCAL_X, KINFU_DEFAULT_DEPTH_FOCAL_Y); // default values, can be overwritten
init_Rcam_ = Eigen::Matrix3f::Identity ();// * AngleAxisf(-30.f/180*3.1415926, Vector3f::UnitX());
init_tcam_ = volume_size * 0.5f - Vector3f (0, 0, volume_size (2) / 2 * 1.2f);
const int iters[] = {10, 5, 4};
std::copy (iters, iters + LEVELS, icp_iterations_);
const float default_distThres = 0.10f; //meters
const float default_angleThres = sin (20.f * 3.14159254f / 180.f);
const float default_tranc_dist = 0.03f; //meters
setIcpCorespFilteringParams (default_distThres, default_angleThres);
tsdf_volume_->setTsdfTruncDist (default_tranc_dist);
allocateBufffers (rows, cols);
rmats_.reserve (30000);
tvecs_.reserve (30000);
reset ();
}
void MulticamFusion::integrateTSDF(const DepthMap& depth_raw, Matrix3frm* rotPtr, Vector3f* transPtr){
Matrix3frm init_Rcam;
Vector3f init_tcam;
if(rotPtr == 0 && transPtr == 0){
//Default Pose
init_Rcam = Eigen::Matrix3f::Identity ();// * AngleAxisf(-30.f/180*3.1415926, Vector3f::UnitX());
//init_tcam = volume_size * 0.5f - Vector3f (0, 0, volume_size (2) / 2 * 1.2f);
//init_tcam = Vector3f(1.5f, 1.5f, -0.3f);
init_tcam = Vector3f(0, 0, -0.0f);
}
else{
init_Rcam = *rotPtr; // [Ri|ti] - pos of camera, i.e.
init_tcam = *transPtr; // transform from camera to global coo space for (i-1)th camera pose
}
Mat33& device_Rcam = device_cast<Mat33> (init_Rcam);
float3& device_tcam = device_cast<float3>(init_tcam);
Matrix3frm init_Rcam_inv = init_Rcam.inverse ();
Mat33& device_Rcam_inv = device_cast<Mat33> (init_Rcam_inv);
float3 device_volume_size = device_cast<const float3>(tsdf_volume_->getSize());
device::Intr intr (525.f, 525.f, 0, 0);
device::integrateTsdfVolume(depth_raw, intr, device_volume_size, device_Rcam_inv, device_tcam,
tsdf_volume_->getTsdfTruncDist(), tsdf_volume_->data(), depthRawScaled_);
}
VectorXd Tools::PolarToCartesian(const VectorXd& x_in) {
/**
* Convert polar coordinates to Cartesian:
* Input is a 3x vector in polar coordinate: rho, theta, rho_dot
* Output is 4x vector in Cartesian coordinates: px, py, vx, vy
*/
VectorXd x_out = VectorXd(4);
x_out.fill(0.0);
float rho = x_in(0);
float theta = x_in(1);
float rho_dot = x_in(2);
// normalize y
float pi_2 = 2*M_PI;
while(theta > M_PI) theta -= pi_2;
while(theta < -M_PI) theta += pi_2;
float px = rho * cos(theta);
float py = rho * sin(theta);
float vx = rho_dot * cos(theta);
float vy = rho_dot * sin(theta);
x_out << px, py, vx, vy;
return x_out;
}
void MoleculeTest::prepareMolecule()
{
Atom *a1 = m_molecule->addAtom();
a1->setPos(Vector3d(0.0, 0.0, 0.0));
Atom *a2 = m_molecule->addAtom();
a2->setPos(Vector3d(1.5, 0.0, 0.0));
Atom *a3 = m_molecule->addAtom();
a3->setPos(Vector3d(0.0, 1.5, 0.0));
Atom *a4 = m_molecule->addAtom();
a4->setPos(Vector3d(0.0, 0.0, 1.5));
Bond *b1 = m_molecule->addBond();
b1->setAtoms(a1->id(), a2->id(), 1);
Bond *b2 = m_molecule->addBond();
b2->setAtoms(a2->id(), a3->id(), 1);
Bond *b3 = m_molecule->addBond();
b3->setAtoms(a3->id(), a4->id(), 1);
}
pcl::gpu::kinfuLS::KinfuTracker::KinfuTracker (const Eigen::Vector3f &volume_size, const float shiftingDistance, int rows, int cols) :
cyclical_( DISTANCE_THRESHOLD, VOLUME_SIZE, VOLUME_X), rows_(rows), cols_(cols), global_time_(0), max_icp_distance_(0), integration_metric_threshold_(0.f), perform_last_scan_ (false), finished_(false), lost_ (false), disable_icp_ (false)
{
//const Vector3f volume_size = Vector3f::Constant (VOLUME_SIZE);
const Vector3i volume_resolution (VOLUME_X, VOLUME_Y, VOLUME_Z);
volume_size_ = volume_size(0);
tsdf_volume_ = TsdfVolume::Ptr ( new TsdfVolume(volume_resolution) );
tsdf_volume_->setSize (volume_size);
shifting_distance_ = shiftingDistance;
// set cyclical buffer values
cyclical_.setDistanceThreshold (shifting_distance_);
cyclical_.setVolumeSize (volume_size_, volume_size_, volume_size_);
setDepthIntrinsics (FOCAL_LENGTH, FOCAL_LENGTH); // default values, can be overwritten
init_Rcam_ = Eigen::Matrix3f::Identity ();// * AngleAxisf(-30.f/180*3.1415926, Vector3f::UnitX());
init_tcam_ = volume_size * 0.5f - Vector3f (0, 0, volume_size (2) / 2 * 1.2f);
const int iters[] = {10, 5, 4};
std::copy (iters, iters + LEVELS, icp_iterations_);
const float default_distThres = 0.10f; //meters
const float default_angleThres = sin (20.f * 3.14159254f / 180.f);
const float default_tranc_dist = 0.03f; //meters
setIcpCorespFilteringParams (default_distThres, default_angleThres);
tsdf_volume_->setTsdfTruncDist (default_tranc_dist);
allocateBufffers (rows, cols);
rmats_.reserve (30000);
tvecs_.reserve (30000);
reset ();
// initialize cyclical buffer
cyclical_.initBuffer(tsdf_volume_);
last_estimated_rotation_= Eigen::Matrix3f::Identity ();
last_estimated_translation_= volume_size * 0.5f - Vector3f (0, 0, volume_size (2) / 2 * 1.2f);
}
void
GLWindow::motion(int x, int y)
{
Mouse m = mouse_;
float delta_x = m.beginx - x, delta_y = m.beginy - y;
if (m.button == GLUT_LEFT_BUTTON && m.state == GLUT_DOWN)
{
if (m.modifiers & GLUT_ACTIVE_SHIFT)
{
//zoom in and out
camera_.setFovY(camera_.fovY() * (1.0f + (delta_y / camera_.vpWidth())));
}
else
{
Eigen::AngleAxisf ry(-delta_x / camera_.vpWidth(), Eigen::Vector3f(0, 1, 0));
Eigen::AngleAxisf rx(-delta_y / camera_.vpHeight(), Eigen::Vector3f(1, 0, 0));
Eigen::Quaternionf qx(rx), qy(ry);
camera_.rotateAroundTarget(qy);
camera_.rotateAroundTarget(qx);
}
}
else if (m.button == GLUT_MIDDLE_BUTTON && m.state == GLUT_DOWN)
{
Vector3f X = camera_.position();
Eigen::Quaternionf q = camera_.orientation();
float factor = 10;
Vector3f dX;
if (m.modifiers & GLUT_ACTIVE_SHIFT)
{
//move along the camera plane normal
dX = q * Vector3f(factor * delta_x / camera_.vpWidth(), 0, factor * (-delta_y) / camera_.vpHeight());
}
else
{
//compute the delta x using the the camera orientation,
//so that the motion is in the plane of the camera.
dX = q * Vector3f(factor * delta_x / camera_.vpWidth(), factor * (-delta_y) / camera_.vpHeight(), 0);
}
camera_.setPosition(X + dX);
}
mouse_.beginx = x;
mouse_.beginy = y;
}
static std::tuple<EllipseGeometry, Eigen::MatrixX2f> generate_cv_fail()
{
using Eigen::Vector2f;
EllipseGeometry geom{Vector2f(1094.5, 1225.16), Vector2f(567.041, 365.318), 0.245385};
Eigen::MatrixX2f data(10, 2);
data <<
924.784, 764.160,
928.388, 615.903,
847.4 , 888.014,
929.406, 741.675,
904.564, 825.605,
926.742, 760.746,
863.479, 873.406,
910.987, 808.863,
929.145, 744.976,
917.474, 791.823;
return std::make_tuple(geom, data);
}
vector<VectorXd> PointsArea::points() const {
unsigned n = ps.size();
vector<VectorXd> vs(n, VectorXd(2));
for (unsigned i = 0; i < n; ++i) {
vs[i][0] = ps[i].x();
vs[i][1] = ps[i].y();
}
return vs;
}
GTEST_TEST(testIK, iiwaIK) {
RigidBodyTree model(
GetDrakePath() + "/examples/kuka_iiwa_arm/urdf/iiwa14.urdf");
// Create a timespan for the constraints. It's not particularly
// meaningful in this test since inverseKin() only tests a single
// point, but the constructors need something.
Vector2d tspan;
tspan << 0, 1;
// Start the robot in the zero configuration (all joints zeroed,
// pointing straight up).
VectorXd q0 = model.getZeroConfiguration();
// Constrain iiwa_link_7 (the end effector) to move 0.58 on the X
// axis and down slightly (to make room for the X axis motion).
Vector3d pos_end;
pos_end << 0.58, 0, 0.77;
const double pos_tol = 0.01;
Vector3d pos_lb = pos_end - Vector3d::Constant(pos_tol);
Vector3d pos_ub = pos_end + Vector3d::Constant(pos_tol);
const int link_7_idx = model.FindBodyIndex("iiwa_link_7");
WorldPositionConstraint wpc(&model, link_7_idx,
Vector3d(0, 0, 0), pos_lb, pos_ub, tspan);
// Constrain iiwa_joint_4 between 0.9 and 1.0.
PostureConstraint pc(&model, tspan);
drake::Vector1d joint_lb(0.9);
drake::Vector1d joint_ub(1.0);
Eigen::VectorXi joint_idx(1);
joint_idx(0) = model.findJoint("iiwa_joint_4")->get_position_start_index();
pc.setJointLimits(joint_idx, joint_lb, joint_ub);
std::vector<RigidBodyConstraint*> constraint_array;
constraint_array.push_back(&wpc);
constraint_array.push_back(&pc);
IKoptions ikoptions(&model);
VectorXd q_sol(model.number_of_positions());
q_sol.setZero();
int info = 0;
std::vector<std::string> infeasible_constraint;
inverseKin(&model, q0, q0, constraint_array.size(), constraint_array.data(),
ikoptions, &q_sol, &info, &infeasible_constraint);
EXPECT_EQ(info, 1);
// Check that our constrained joint is within where we tried to constrain it.
EXPECT_GE(q_sol(joint_idx(0)), joint_lb(0));
EXPECT_LE(q_sol(joint_idx(0)), joint_ub(0));
// Check that the link we were trying to position wound up where we expected.
KinematicsCache<double> cache = model.doKinematics(q_sol);
EXPECT_TRUE(CompareMatrices(
pos_end, model.relativeTransform(cache, 0, link_7_idx).translation(),
pos_tol + 1e-6, MatrixCompareType::absolute));
}
void MulticamFusion::show(Eigen::Affine3f* pose_ptr){
device::Intr intr (525.f, 525.f, 0, 0);
//Ray-Casting
int pyr_rows = 480; int pyr_cols = 640;
depthRawScaled_.create (480, 640);
vmaps_g_prev_.create (pyr_rows*3, pyr_cols);
nmaps_g_prev_.create (pyr_rows*3, pyr_cols);
view_device_.create (480, 640);
if(pose_ptr != 0){
raycaster_ptr_->run(*tsdf_volume_, *pose_ptr);
raycaster_ptr_->generateSceneView(view_device_);
}
else{
//Generate Images
Eigen::Vector3f light_source_pose = tsdf_volume_->getSize() * (-3.f);
device::LightSource light;
light.number = 1;
light.pos[0] = device_cast<const float3>(light_source_pose);
float3 device_volume_size = device_cast<const float3>(tsdf_volume_->getSize());
//Viewer's Pose
Matrix3frm tempR = Eigen::Matrix3f::Identity ();
//Vector3f tempT = Vector3f(1.5f, 1.5f, -0.3f);
Vector3f tempT = Vector3f(0.09f, 0.29f, 0.14f);
Mat33& device_Rcam = device_cast<Mat33> (tempR);
float3& device_tcam = device_cast<float3>(tempT);
raycast (intr, device_Rcam, device_tcam, tsdf_volume_->getTsdfTruncDist(), device_volume_size,
tsdf_volume_->data(), vmaps_g_prev_, nmaps_g_prev_);
generateImage (vmaps_g_prev_, nmaps_g_prev_, light, view_device_);
}
getLastFrameCloud(*cloud_device_);
int c;
cloud_device_->download (cloud_ptr_->points, c);
cloud_ptr_->width = cloud_device_->cols ();
cloud_ptr_->height = cloud_device_->rows ();
cloud_ptr_->is_dense = false;
cloud_viewer_.removeAllPointClouds ();
cloud_viewer_.addPointCloud<PointXYZ>(cloud_ptr_);
//For Getting Pose
cloud_viewer_.spinOnce();
//Display
int cols;
view_device_.download(viewer_host_, cols);
image_viewer_.showRGBImage ((unsigned char*)&viewer_host_[0], view_device_.cols (), view_device_.rows ());
image_viewer_.spinOnce();
}
Vector3d Compound::Point::getVector(Manifold* space) {
for(int i=0; i<5; ++i) {
assert(getPosition()->isInManifold());
std::pair<bool,Vector3d> out = getVector(space, i);
if(out.first) {
//std::cout << "Compound.cpp Vector:\t" << out.second << std::endl;
return out.second;
}
}
std::cout << "Compound.cpp No connection to space found." << std::endl;
return Vector3d(0,0,0);
}
void MoleculeTest::translate()
{
m_molecule->translate(Vector3d(1.0, 1.1, 1.2));
QCOMPARE(m_molecule->atom(0)->pos()->x(), 1.0);
QCOMPARE(m_molecule->atom(0)->pos()->y(), 1.1);
QCOMPARE(m_molecule->atom(0)->pos()->z(), 1.2);
QCOMPARE(m_molecule->atom(1)->pos()->x(), 2.5);
// Check the center was correctly updated
QCOMPARE(m_molecule->center().x(), 1.5 / 4.0 + 1.0);
QCOMPARE(m_molecule->center().y(), 1.5 / 4.0 + 1.1);
QCOMPARE(m_molecule->center().z(), 1.5 / 4.0 + 1.2);
}
