/** * 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); }