void paramsCallback(navigation::ForceFieldConfig &config, uint32_t level)
{
ROS_DEBUG_STREAM("Force field reconfigure Request ->" << " kp_x:" << config.kp_x
<< " kp_y:" << config.kp_y
<< " kp_z:" << config.kp_z
<< " kd_x:" << config.kd_x
<< " kd_y:" << config.kd_y
<< " kd_z:" << config.kd_z
<< " ro:" << config.ro);
kp << config.kp_x,
config.kp_y,
config.kp_z;
kd << config.kd_x,
config.kd_y,
config.kd_z;
kp_mat << kp.x(), 0, 0,
0, kp.y(), 0,
0, 0, kp.z();
kd_mat << kd.x(), 0, 0,
0, kd.y(), 0,
0, 0, kd.z();
ro = config.ro;
laser_max_distance = config.laser_max_distance;
//slave_to_master_scale=Eigen::Matrix<double,3,1> (fabs(config.master_workspace_size.x/config.slave_workspace_size.x), fabs(config.master_workspace_size.y/config.slave_workspace_size.y), fabs(config.master_workspace_size.z/config.slave_workspace_size.z));
}
/**
* @brief Send a safety zone (volume), which is defined by two corners of a cube,
* to the FCU.
*
* @note ENU frame.
*/
void send_safety_set_allowed_area(Eigen::Vector3d p1, Eigen::Vector3d p2)
{
ROS_INFO_STREAM_NAMED("safetyarea", "SA: Set safty area: P1 " << p1 << " P2 " << p2);
p1 = ftf::transform_frame_enu_ned(p1);
p2 = ftf::transform_frame_enu_ned(p2);
mavlink::common::msg::SAFETY_SET_ALLOWED_AREA s;
m_uas->msg_set_target(s);
// TODO: use enum from lib
s.frame = utils::enum_value(mavlink::common::MAV_FRAME::LOCAL_NED);
// [[[cog:
// for p in range(1, 3):
// for v in ('x', 'y', 'z'):
// cog.outl("s.p%d%s = p%d.%s();" % (p, v, p, v))
// ]]]
s.p1x = p1.x();
s.p1y = p1.y();
s.p1z = p1.z();
s.p2x = p2.x();
s.p2y = p2.y();
s.p2z = p2.z();
// [[[end]]] (checksum: c996a362f338fcc6b714c8be583c3be0)
UAS_FCU(m_uas)->send_message_ignore_drop(s);
}
BoundingBox Face::boundingBox() const
{
if (isBoundary()) {
return BoundingBox(he->vertex->position, he->next->vertex->position);
}
Eigen::Vector3d p1 = he->vertex->position;
Eigen::Vector3d p2 = he->next->vertex->position;
Eigen::Vector3d p3 = he->next->next->vertex->position;
Eigen::Vector3d min = p1;
Eigen::Vector3d max = p1;
if (p2.x() < min.x()) min.x() = p2.x();
if (p3.x() < min.x()) min.x() = p3.x();
if (p2.y() < min.y()) min.y() = p2.y();
if (p3.y() < min.y()) min.y() = p3.y();
if (p2.z() < min.z()) min.z() = p2.z();
if (p3.z() < min.z()) min.z() = p3.z();
if (p2.x() > max.x()) max.x() = p2.x();
if (p3.x() > max.x()) max.x() = p3.x();
if (p2.y() > max.y()) max.y() = p2.y();
if (p3.y() > max.y()) max.y() = p3.y();
if (p2.z() > max.z()) max.z() = p2.z();
if (p3.z() > max.z()) max.z() = p3.z();
return BoundingBox(min, max);
}
void GlobalOptimization::inputOdom(double t, Eigen::Vector3d OdomP, Eigen::Quaterniond OdomQ)
{
mPoseMap.lock();
vector<double> localPose{OdomP.x(), OdomP.y(), OdomP.z(),
OdomQ.w(), OdomQ.x(), OdomQ.y(), OdomQ.z()};
localPoseMap[t] = localPose;
Eigen::Quaterniond globalQ;
globalQ = WGPS_T_WVIO.block<3, 3>(0, 0) * OdomQ;
Eigen::Vector3d globalP = WGPS_T_WVIO.block<3, 3>(0, 0) * OdomP + WGPS_T_WVIO.block<3, 1>(0, 3);
vector<double> globalPose{globalP.x(), globalP.y(), globalP.z(),
globalQ.w(), globalQ.x(), globalQ.y(), globalQ.z()};
globalPoseMap[t] = globalPose;
lastP = globalP;
lastQ = globalQ;
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header.stamp = ros::Time(t);
pose_stamped.header.frame_id = "world";
pose_stamped.pose.position.x = lastP.x();
pose_stamped.pose.position.y = lastP.y();
pose_stamped.pose.position.z = lastP.z();
pose_stamped.pose.orientation.x = lastQ.x();
pose_stamped.pose.orientation.y = lastQ.y();
pose_stamped.pose.orientation.z = lastQ.z();
pose_stamped.pose.orientation.w = lastQ.w();
global_path.header = pose_stamped.header;
global_path.poses.push_back(pose_stamped);
mPoseMap.unlock();
}
void FeatureTracker::showUndistortion(const string &name)
{
cv::Mat undistortedImg(ROW + 600, COL + 600, CV_8UC1, cv::Scalar(0));
vector<Eigen::Vector2d> distortedp, undistortedp;
for (int i = 0; i < COL; i++)
for (int j = 0; j < ROW; j++)
{
Eigen::Vector2d a(i, j);
Eigen::Vector3d b;
m_camera->liftProjective(a, b);
distortedp.push_back(a);
undistortedp.push_back(Eigen::Vector2d(b.x() / b.z(), b.y() / b.z()));
//printf("%f,%f->%f,%f,%f\n)\n", a.x(), a.y(), b.x(), b.y(), b.z());
}
for (int i = 0; i < int(undistortedp.size()); i++)
{
cv::Mat pp(3, 1, CV_32FC1);
pp.at<float>(0, 0) = undistortedp[i].x() * FOCAL_LENGTH + COL / 2;
pp.at<float>(1, 0) = undistortedp[i].y() * FOCAL_LENGTH + ROW / 2;
pp.at<float>(2, 0) = 1.0;
//cout << trackerData[0].K << endl;
//printf("%lf %lf\n", p.at<float>(1, 0), p.at<float>(0, 0));
//printf("%lf %lf\n", pp.at<float>(1, 0), pp.at<float>(0, 0));
if (pp.at<float>(1, 0) + 300 >= 0 && pp.at<float>(1, 0) + 300 < ROW + 600 && pp.at<float>(0, 0) + 300 >= 0 && pp.at<float>(0, 0) + 300 < COL + 600)
{
undistortedImg.at<uchar>(pp.at<float>(1, 0) + 300, pp.at<float>(0, 0) + 300) = cur_img.at<uchar>(distortedp[i].y(), distortedp[i].x());
}
else
{
//ROS_ERROR("(%f %f) -> (%f %f)", distortedp[i].y, distortedp[i].x, pp.at<float>(1, 0), pp.at<float>(0, 0));
}
}
cv::imshow(name, undistortedImg);
cv::waitKey(0);
}
Eigen::Vector3d PIDController::compute_linvel_effort(Eigen::Vector3d goal, Eigen::Vector3d current, ros::Time last_time){
double lin_vel_x = pid_linvel_x.computeCommand(goal.x() - current.x(), ros::Time::now() - last_time);
double lin_vel_y = pid_linvel_y.computeCommand(goal.y() - current.y(), ros::Time::now() - last_time);
double lin_vel_z = pid_linvel_z.computeCommand(goal.z() - current.z(), ros::Time::now() - last_time);
return Eigen::Vector3d(lin_vel_x, lin_vel_y, lin_vel_z);
}
bool RobotLaser::write(std::ostream& os) const
{
os << _laserParams.type << " " << _laserParams.firstBeamAngle << " " << _laserParams.fov << " "
<< _laserParams.angularStep << " " << _laserParams.maxRange << " " << _laserParams.accuracy << " "
<< _laserParams.remissionMode << " ";
os << ranges().size();
for (size_t i = 0; i < ranges().size(); ++i)
os << " " << ranges()[i];
os << " " << _remissions.size();
for (size_t i = 0; i < _remissions.size(); ++i)
os << " " << _remissions[i];
// odometry pose
Eigen::Vector3d p = (_odomPose * _laserParams.laserPose).toVector();
os << " " << p.x() << " " << p.y() << " " << p.z();
p = _odomPose.toVector();
os << " " << p.x() << " " << p.y() << " " << p.z();
// crap values
os << FIXED(" " << _laserTv << " " << _laserRv << " " << _forwardSafetyDist << " "
<< _sideSaftyDist << " " << _turnAxis);
os << FIXED(" " << timestamp() << " " << hostname() << " " << loggerTimestamp());
return os.good();
}
//! Calculate geodetic coordinates ( altitude, geodetic latitude, longitude ) of a position vector.
Eigen::Vector3d convertCartesianToGeodeticCoordinates( const Eigen::Vector3d cartesianCoordinates,
const double equatorialRadius,
const double flattening,
const double tolerance )
{
using coordinate_conversions::convertCartesianToSpherical;
// Determine spherical coordinates of position vector.
const Eigen::Vector3d sphericalCoordinates
= convertCartesianToSpherical( cartesianCoordinates );
Eigen::Vector3d geodeticCoordinates = Eigen::Vector3d::Zero( );
// Calculate auxiliary variables of geodetic coordinates.
std::pair< double, double > auxiliaryVariables =
calculateGeodeticCoordinatesAuxiliaryQuantities(
cartesianCoordinates, equatorialRadius,
calculateEllipticity( flattening ), tolerance );
// Calculate altitude.
geodeticCoordinates.x( ) = calculateAltitudeOverOblateSpheroid(
cartesianCoordinates, auxiliaryVariables.second, auxiliaryVariables.first );
// Calculate geodetic latitude.
geodeticCoordinates.y( ) = calculateGeodeticLatitude(
cartesianCoordinates, auxiliaryVariables.second );
// Set longitude.
geodeticCoordinates.z( ) = sphericalCoordinates.z( );
return geodeticCoordinates;
}
inline void BasisSet::pointD(BasisSet *set, const Eigen::Vector3d &delta,
const double &dr2, int basis,
Eigen::MatrixXd &out)
{
// D type orbitals have six components and each component has a different
// independent MO weighting. Many things can be cached to save time though
double xx = 0.0, yy = 0.0, zz = 0.0, xy = 0.0, xz = 0.0, yz = 0.0;
// Now iterate through the D type GTOs and sum their contributions
unsigned int cIndex = set->m_cIndices[basis];
for (unsigned int i = set->m_gtoIndices[basis];
i < set->m_gtoIndices[basis+1]; ++i) {
// Calculate the common factor
double tmpGTO = exp(-set->m_gtoA[i] * dr2);
xx += set->m_gtoCN[cIndex++] * tmpGTO; // Dxx
yy += set->m_gtoCN[cIndex++] * tmpGTO; // Dyy
zz += set->m_gtoCN[cIndex++] * tmpGTO; // Dzz
xy += set->m_gtoCN[cIndex++] * tmpGTO; // Dxy
xz += set->m_gtoCN[cIndex++] * tmpGTO; // Dxz
yz += set->m_gtoCN[cIndex++] * tmpGTO; // Dyz
}
// Save values to the matrix
int baseIndex = set->m_moIndices[basis];
out.coeffRef(baseIndex , 0) = delta.x() * delta.x() * xx;
out.coeffRef(baseIndex+1, 0) = delta.y() * delta.y() * yy;
out.coeffRef(baseIndex+2, 0) = delta.z() * delta.z() * zz;
out.coeffRef(baseIndex+3, 0) = delta.x() * delta.y() * xy;
out.coeffRef(baseIndex+4, 0) = delta.x() * delta.z() * xz;
out.coeffRef(baseIndex+5, 0) = delta.y() * delta.z() * yz;
}
inline VoxelGrid::GRID_INDEX GetNextFromGradient(const VoxelGrid::GRID_INDEX& index, const Eigen::Vector3d& gradient) const
{
// Given the gradient, pick the "best fit" of the 26 neighboring points
VoxelGrid::GRID_INDEX next_index = index;
double half_resolution = GetResolution() * 0.5;
if (gradient.x() > half_resolution)
{
next_index.x++;
}
else if (gradient.x() < -half_resolution)
{
next_index.x--;
}
if (gradient.y() > half_resolution)
{
next_index.y++;
}
else if (gradient.y() < -half_resolution)
{
next_index.y--;
}
if (gradient.z() > half_resolution)
{
next_index.z++;
}
else if (gradient.z() < -half_resolution)
{
next_index.z--;
}
return next_index;
}
void draw()
{
glColor4f(0.0, 0.0, 1.0, 0.5);
glLineWidth(1.0);
glBegin(GL_LINES);
for (EdgeCIter e = mesh.edges.begin(); e != mesh.edges.end(); e ++) {
Eigen::Vector3d a = e->he->vertex->position;
Eigen::Vector3d b = e->he->flip->vertex->position;
glVertex3d(a.x(), a.y(), a.z());
glVertex3d(b.x(), b.y(), b.z());
}
glEnd();
for (VertexIter v = mesh.vertices.begin(); v != mesh.vertices.end(); v++) {
if (v->handle) {
glColor4f(0.0, 1.0, 0.0, 0.5);
glPointSize(4.0);
glBegin(GL_POINTS);
glVertex3d(v->position.x(), v->position.y(), v->position.z());
glEnd();
}
if (v->anchor) {
glColor4f(1.0, 0.0, 0.0, 0.5);
glPointSize(2.0);
glBegin(GL_POINTS);
glVertex3d(v->position.x(), v->position.y(), v->position.z());
glEnd();
}
}
}
void mesh_core::Plane::leastSquaresGeneral(
const EigenSTL::vector_Vector3d& points,
Eigen::Vector3d* average)
{
if (points.empty())
{
normal_ = Eigen::Vector3d(0,0,1);
d_ = 0;
if (average)
*average = Eigen::Vector3d::Zero();
return;
}
// find c, the average of the points
Eigen::Vector3d c;
c.setZero();
EigenSTL::vector_Vector3d::const_iterator p = points.begin();
EigenSTL::vector_Vector3d::const_iterator end = points.end();
for ( ; p != end ; ++p)
c += *p;
c *= 1.0/double(points.size());
// Find the matrix
Eigen::Matrix3d m;
m.setZero();
p = points.begin();
for ( ; p != end ; ++p)
{
Eigen::Vector3d cp = *p - c;
m(0,0) += cp.x() * cp.x();
m(1,0) += cp.x() * cp.y();
m(2,0) += cp.x() * cp.z();
m(1,1) += cp.y() * cp.y();
m(2,1) += cp.y() * cp.z();
m(2,2) += cp.z() * cp.z();
}
m(0,1) = m(1,0);
m(0,2) = m(2,0);
m(1,2) = m(2,1);
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(m);
if (eigensolver.info() == Eigen::Success)
{
normal_ = eigensolver.eigenvectors().col(0);
normal_.normalize();
}
else
{
normal_ = Eigen::Vector3d(0,0,1);
}
d_ = -c.dot(normal_);
if (average)
*average = c;
}
/**
* \ingroup Communication
* Move the Object at absolute position given a Optotrak coordinate system (calibration) synchronously (blocking call)
*
* \param point Final absolute point to reach
* \param calibration Current calibration of Optotrak coordinate system
* \param objectspeed Object speed
**/
void moveObjectAbsolute(const Eigen::Vector3d &point, const Eigen::Vector3d &calibration, int objectspeed)
{
int nstepsY = -(point.y()-calibration.y())/BROWN_MOTORSTEPSIZE;
int nstepsZ = (point.z()-calibration.z())/BROWN_MOTORSTEPSIZE;
cerr << "Object is brought to " << point.y() << " Y and " << point.z() << " Z" << endl;
}
double errorOfLineInPlane(Segment_3 &l, Eigen::Vector3d& point, Eigen::Vector3d& normal)
{
Plane_3 plane( Point_3(point.x(), point.y(), point.z()),
Vector_3(normal.x(), normal.y(), normal.z()) );
double dist1 = squared_distance(plane, l.point(0));
double dist2 = squared_distance(plane, l.point(1));
return 0.5*(sqrt(dist1)+sqrt(dist2));
}
inline Eigen::Matrix3d _skew(const Eigen::Vector3d& t){
Eigen::Matrix3d S;
S <<
0, -t.z(), t.y(),
t.z(), 0, -t.x(),
-t.y(), t.x(), 0;
return S;
}
Eigen::Vector3d MetricRectification::normalizeLine(Eigen::Vector3d p0, Eigen::Vector3d p1)
{
Eigen::Vector3d l = p0.cross(p1);
l.x() = l.x() / l.z();
l.y() = l.y() / l.z();
l.z() = 1.0;
//return l;
return p0.cross(p1).normalized();
}
double errorOfCoplanar(Eigen::Vector3d &pt1, Eigen::Vector3d &normal1, Eigen::Vector3d &pt2, Eigen::Vector3d &normal2)
{
double err1 = 1-std::abs(normal1.dot(normal2));
Point_3 point(pt1.x(), pt1.y(), pt1.z());
Plane_3 p2(Point_3(pt2.x(), pt2.y(), pt2.z()), Vector_3(normal2.x(), normal2.y(), normal2.z()));
double dist1 = squared_distance(p2, point);
return err1 + sqrt(dist1);
}
bool BoundingBox::intersect(const Eigen::Vector3d& origin, const Eigen::Vector3d& direction,
double& dist) const
{
double ox = origin.x();
double dx = direction.x();
double tMin, tMax;
if (dx >= 0) {
tMin = (min.x() - ox) / dx;
tMax = (max.x() - ox) / dx;
} else {
tMin = (max.x() - ox) / dx;
tMax = (min.x() - ox) / dx;
}
double oy = origin.y();
double dy = direction.y();
double tyMin, tyMax;
if (dy >= 0) {
tyMin = (min.y() - oy) / dy;
tyMax = (max.y() - oy) / dy;
} else {
tyMin = (max.y() - oy) / dy;
tyMax = (min.y() - oy) / dy;
}
if (tMin > tyMax || tyMin > tMax) {
return false;
}
if (tyMin > tMin) tMin = tyMin;
if (tyMax < tMax) tMax = tyMax;
double oz = origin.z();
double dz = direction.z();
double tzMin, tzMax;
if (dz >= 0) {
tzMin = (min.z() - oz) / dz;
tzMax = (max.z() - oz) / dz;
} else {
tzMin = (max.z() - oz) / dz;
tzMax = (min.z() - oz) / dz;
}
if (tMin > tzMax || tzMin > tMax) {
return false;
}
if (tzMin > tMin) tMin = tzMin;
if (tzMax < tMax) tMax = tzMax;
dist = tMin;
return true;
}
VirtualImpedanceForce::VirtualImpedanceForce(ros::NodeHandle & n_ , Eigen::Vector3d kp , Eigen::Vector3d kd):ForceField(n_)
{
kp_mat << kp.x(), 0, 0,
0, kp.y(), 0,
0, 0, kp.z();
kd_mat << kd.x(), 0, 0,
0, kd.y(), 0,
0, 0, kd.z();
std::cout << "child constructor" << std::endl;
}
void RotateSelectionDialog::setAxis(const Eigen::Vector3d &axis,
const Eigen::Vector3d &offset)
{
ui.spin_vx->setValue(axis.x());
ui.spin_vy->setValue(axis.y());
ui.spin_vz->setValue(axis.z());
ui.spin_tx->setValue(offset.x());
ui.spin_ty->setValue(offset.y());
ui.spin_tz->setValue(offset.z());
}
// helper function to avoid code duplication
static inline kinematic_constraints::ConstraintEvaluationResult finishPositionConstraintDecision(const Eigen::Vector3d &pt, const Eigen::Vector3d &desired, const std::string &name,
double weight, bool result, bool verbose)
{
if (verbose)
ROS_INFO("Position constraint %s on link '%s'. Desired: %f, %f, %f, current: %f, %f, %f",
result ? "satisfied" : "violated", name.c_str(), desired.x(), desired.y(), desired.z(), pt.x(), pt.y(), pt.z());
double dx = desired.x() - pt.x();
double dy = desired.y() - pt.y();
double dz = desired.z() - pt.z();
return ConstraintEvaluationResult(result, weight * sqrt(dx * dx + dy * dy + dz * dz));
}
/**
* \ingroup Communication
* Move the Object on Z,Y axis synchronously
*
* \param delta Object displacement
* \param speed Object speed
**/
void moveObject(const Eigen::Vector3d &delta, int speed)
{
if ( delta.norm() > 1e20 )
{ std::cerr << "Impossible position to reach! Check marker visibility!" << endl;
return;
}
Eigen::Vector3d displacement(0,-delta.y(),delta.z());
cerr << "Object moves " << -delta.y() << " vertically." << endl;
cerr << "Object moves " << delta.z() << " in depth." << endl;
}
void
Execute (vtkObject *caller, unsigned long vtkNotUsed(eventId), void* vtkNotUsed(callData))
{
vtkRenderWindowInteractor *interactor = vtkRenderWindowInteractor::SafeDownCast (caller);
vtkRenderer *renderer = interactor->FindPokedRenderer (interactor->GetEventPosition ()[0],
interactor->GetEventPosition ()[1]);
std::string key (interactor->GetKeySym ());
bool shift_down = interactor->GetShiftKey();
cout << "Key Pressed: " << key << endl;
Scene *scene = Scene::instance ();
OutofcoreCloud *cloud = static_cast<OutofcoreCloud*> (scene->getObjectByName ("my_octree"));
if (key == "Up" || key == "Down")
{
if (key == "Up" && cloud)
{
if (shift_down)
{
cloud->increaseLodPixelThreshold();
}
else
{
cloud->setDisplayDepth (cloud->getDisplayDepth () + 1);
}
}
else if (key == "Down" && cloud)
{
if (shift_down)
{
cloud->decreaseLodPixelThreshold();
}
else
{
cloud->setDisplayDepth (cloud->getDisplayDepth () - 1);
}
}
}
if (key == "o")
{
cloud->setDisplayVoxels(1-static_cast<int> (cloud->getDisplayVoxels()));
}
if (key == "Escape")
{
Eigen::Vector3d min (cloud->getBoundingBoxMin ());
Eigen::Vector3d max (cloud->getBoundingBoxMax ());
renderer->ResetCamera (min.x (), max.x (), min.y (), max.y (), min.z (), max.z ());
}
}
void BoundingBox::expandToInclude(const Eigen::Vector3d& p)
{
if (min.x() > p.x()) min.x() = p.x();
if (min.y() > p.y()) min.y() = p.y();
if (min.z() > p.z()) min.z() = p.z();
if (max.x() < p.x()) max.x() = p.x();
if (max.y() < p.y()) max.y() = p.y();
if (max.z() < p.z()) max.z() = p.z();
extent = max - min;
}
void display()
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
GLint viewport[4];
glGetIntegerv(GL_VIEWPORT, viewport);
double aspect = (double)viewport[2] / (double)viewport[3];
gluPerspective(fovy, aspect, clipNear, clipFar);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
gluLookAt(0, 0, z, x, y, 0, 0, 1, 0);
if (success) {
drawFaces();
if (drawAABB) {
Eigen::Vector3d max = boundingBox.max;
Eigen::Vector3d min = boundingBox.min;
Eigen::Vector3d extent = boundingBox.extent;
Eigen::Vector3d b2(min.x() + extent.x(), min.y(), min.z());
Eigen::Vector3d b3(min.x() + extent.x(), min.y() + extent.y(), min.z());
Eigen::Vector3d b4(min.x(), min.y() + extent.y(), min.z());
Eigen::Vector3d b5(max.x() - extent.x(), max.y() - extent.y(), max.z());
Eigen::Vector3d b6(max.x(), max.y() - extent.y(), max.z());
Eigen::Vector3d b8(max.x() - extent.x(), max.y(), max.z());
drawBox(min, b2, b3, b4, b5, b6, max, b8);
} else {
std::vector<Eigen::Vector3d> orientedPoints = boundingBox.orientedPoints;
Eigen::Vector3d b1 = orientedPoints[0] + orientedPoints[2] + orientedPoints[4];
Eigen::Vector3d b2 = orientedPoints[1] + orientedPoints[2] + orientedPoints[4];
Eigen::Vector3d b3 = orientedPoints[1] + orientedPoints[2] + orientedPoints[5];
Eigen::Vector3d b4 = orientedPoints[0] + orientedPoints[2] + orientedPoints[5];
Eigen::Vector3d b5 = orientedPoints[0] + orientedPoints[3] + orientedPoints[4];
Eigen::Vector3d b6 = orientedPoints[1] + orientedPoints[3] + orientedPoints[4];
Eigen::Vector3d b8 = orientedPoints[0] + orientedPoints[3] + orientedPoints[5];
Eigen::Vector3d b7 = orientedPoints[1] + orientedPoints[3] + orientedPoints[5];
drawBox(b1, b2, b3, b4, b5, b6, b7, b8);
}
}
glutSwapBuffers();
}
vector<cv::Point2f> FeatureTracker::undistortedPoints()
{
vector<cv::Point2f> un_pts;
//cv::undistortPoints(cur_pts, un_pts, K, cv::Mat());
for (unsigned int i = 0; i < cur_pts.size(); i++)
{
Eigen::Vector2d a(cur_pts[i].x, cur_pts[i].y);
Eigen::Vector3d b;
m_camera->liftProjective(a, b);
un_pts.push_back(cv::Point2f(b.x() / b.z(), b.y() / b.z()));
}
return un_pts;
}
inline void readNextMessageFromFile( std::ifstream& p_ifMessages, const std::string& p_strImageFolder )
{
//ds line buffer
std::string strLineBuffer;
//ds read one line
std::getline( p_ifMessages, strLineBuffer );
if( strLineBuffer.empty( ) ){ throw CExceptionEndOfFile( "received empty string - file end reached" ); }
//ds get it to a stringstream
std::istringstream issLine( strLineBuffer );
//ds information fields
double dTimeSeconds;
std::string strToken;
std::string strMessageType;
//ds fetch first part of the message
issLine >> strToken >> strToken >> dTimeSeconds >> strMessageType;
//ds get time to seconds
dTimeSeconds /= 1000000;
//ds set message information depending on type
if( "IMU" == strMessageType )
{
//ds IMU message
txt_io::CIMUMessage msgIMU( "/imu", "imu", g_uFrameIDIMU, dTimeSeconds );
//ds fields
Eigen::Vector3d vecAngularVelocity;
CLinearAccelerationIMU vecLinearAcceleration;
//ds parse the values (order x/z/y) TODO align coordinate systems
issLine >> strToken >> vecLinearAcceleration[0] >> vecLinearAcceleration[1] >> vecLinearAcceleration[2] >> vecAngularVelocity[0] >> vecAngularVelocity[1] >> vecAngularVelocity[2];
//ds rotate around X axis by 180 degrees
vecLinearAcceleration.y( ) = -vecLinearAcceleration.y( );
vecLinearAcceleration.z( ) = -vecLinearAcceleration.z( );
vecAngularVelocity.y( ) = -vecAngularVelocity.y( );
vecAngularVelocity.z( ) = -vecAngularVelocity.z( );
//ds set message fields
msgIMU.setAngularVelocity( vecAngularVelocity );
msgIMU.setLinearAcceleration( vecLinearAcceleration );
//ds pump it into the synchronizer
g_vecMessagesIMU.push( msgIMU );
}
static Eigen::Vector3d mapToSphere(
Eigen::Matrix4d invProjMatrix,
Eigen::Vector2d const p,
Eigen::Vector3d viewSphereCenter,
double viewSpaceRadius) {
Eigen::Vector4d viewP = invProjMatrix * Eigen::Vector4d(p(0), p(1), -1.0, 1.0);
//viewP(0) /= viewP(3);
//viewP(1) /= viewP(3);
//viewP(2) /= viewP(3);
Eigen::Vector3d rayOrigin(0.0, 0.0, 0.0);
Eigen::Vector3d rayDir(viewP(0), viewP(1), viewP(2));
rayDir.normalize();
Eigen::Vector3d CO = rayOrigin - viewSphereCenter;
double a = 1.0;
double bPrime = CO.dot(rayDir);
double c = CO.dot(CO) - viewSpaceRadius * viewSpaceRadius;
double delta = bPrime * bPrime - a * c;
if (delta < 0.0) {
double t = -CO.z() / rayDir.z();
Eigen::Vector3d point = rayOrigin + t * rayDir;
Eigen::Vector3d dir = point - viewSphereCenter;
dir.normalize();
return viewSphereCenter + dir * viewSpaceRadius;
}
double root[2] = { (-bPrime - sqrt(delta)) / a, (-bPrime + sqrt(delta)) / a };
if (root[0] >= 0.0) {
Eigen::Vector3d intersectionPoint = rayOrigin + root[0] * rayDir;
return intersectionPoint;
}
else if (root[1] >= 0.0) {
Eigen::Vector3d intersectionPoint = rayOrigin + root[1] * rayDir;
return intersectionPoint;
}
else {
double t = -CO.z() / rayDir.z();
Eigen::Vector3d point = rayOrigin + t * rayDir;
Eigen::Vector3d dir = point - viewSphereCenter;
dir.normalize();
return viewSphereCenter + dir * viewSpaceRadius;
}
}
void Controller::reach() {
// Change to reaching pose
Eigen::VectorXd newPose;
Eigen::Vector3d position;
mDesiredDofs = mDefaultPose;
mDesiredDofs[6] = 0.2;
mDesiredDofs[9] = 0.2;
mDesiredDofs[14] = -0.2;
mDesiredDofs[15] = -0.2;
mDesiredDofs[17] = -0.2;
mDesiredDofs[19] = -0.2;
mDesiredDofs[27] = 0.7;
mDesiredDofs[28] = -2.5;
mDesiredDofs[30] = 0.7;
mDesiredDofs[31] = 2.5;
mDesiredDofs[33] = 0.4;
mDesiredDofs[34] = 0.4;
if(numBars == 1){
position = barPositions[0];
}
else{
position = barPositions[1];
}
position.z() = -1.0 * (handDiffZ/2.0);
newPose = ik(mSkel->getBodyNode("h_hand_left"), position);
mDesiredDofs += (newPose - mSkel->getPositions());
position.z() = 1.0 * (handDiffZ/2.0);
newPose = ik(mSkel->getBodyNode("h_hand_right"), position);
mDesiredDofs += (newPose - mSkel->getPositions());
stablePD();
checkContactState();
if (mFootContact) { // If feet are in contact again, go back to JUMP and continue to push
mState = "JUMP";
std::cout << mCurrentFrame << ": " << "REACH -> JUMP" << std::endl;
} else if (mLeftHandContact || mRightHandContact) {
mState = "GRAB";
mTimer = 500;
std::cout << mCurrentFrame << ": " << "REACH -> GRAB" << std::endl;
} else {
mState = "REACH";
}
}
void FeatureTracker::undistortedPoints()
{
cur_un_pts.clear();
cur_un_pts_map.clear();
//cv::undistortPoints(cur_pts, un_pts, K, cv::Mat());
for (unsigned int i = 0; i < cur_pts.size(); i++)
{
Eigen::Vector2d a(cur_pts[i].x, cur_pts[i].y);
Eigen::Vector3d b;
m_camera->liftProjective(a, b);
cur_un_pts.push_back(cv::Point2f(b.x() / b.z(), b.y() / b.z()));
cur_un_pts_map.insert(make_pair(ids[i], cv::Point2f(b.x() / b.z(), b.y() / b.z())));
//printf("cur pts id %d %f %f", ids[i], cur_un_pts[i].x, cur_un_pts[i].y);
}
// caculate points velocity
if (!prev_un_pts_map.empty())
{
double dt = cur_time - prev_time;
pts_velocity.clear();
for (unsigned int i = 0; i < cur_un_pts.size(); i++)
{
if (ids[i] != -1)
{
std::map<int, cv::Point2f>::iterator it;
it = prev_un_pts_map.find(ids[i]);
if (it != prev_un_pts_map.end())
{
double v_x = (cur_un_pts[i].x - it->second.x) / dt;
double v_y = (cur_un_pts[i].y - it->second.y) / dt;
pts_velocity.push_back(cv::Point2f(v_x, v_y));
}
else
pts_velocity.push_back(cv::Point2f(0, 0));
}
else
{
pts_velocity.push_back(cv::Point2f(0, 0));
}
}
}
else
{
for (unsigned int i = 0; i < cur_pts.size(); i++)
{
pts_velocity.push_back(cv::Point2f(0, 0));
}
}
prev_un_pts_map = cur_un_pts_map;
}
