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();
}
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);
}
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();
}
}
}
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;
}
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;
}
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();
}
partition::extents_type expanded_( const partition::extents_type& extents, const Eigen::Vector3d& resolution )
{
Eigen::Vector3d floor = extents.min() - resolution / 2;
Eigen::Vector3d ceil = extents.max() + resolution / 2;
return partition::extents_type( Eigen::Vector3d( std::floor( floor.x() ), std::floor( floor.y() ), std::floor( floor.z() ) )
, Eigen::Vector3d( std::ceil( ceil.x() ), std::ceil( ceil.y() ), std::ceil( ceil.z() ) ) );
}
int TextRenderer::draw( const Eigen::Vector3d &pos, const QString &string )
{
assert(d->textmode);
if( string.isEmpty() ) return 0;
const QFontMetrics fontMetrics ( d->font );
int w = fontMetrics.width(string);
int h = fontMetrics.height();
Eigen::Vector3d wincoords = d->glwidget->camera()->project(pos);
// project is in QT window coordinates
wincoords.y() = d->glwidget->height() - wincoords.y();
wincoords.x() -= w/2;
wincoords.y() += h/2;
glPushMatrix();
glLoadIdentity();
glTranslatef( static_cast<int>(wincoords.x()),
static_cast<int>(wincoords.y()),
-wincoords.z() );
d->do_draw(string);
glPopMatrix();
return h;
}
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;
}
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 Camera::unProject(const Eigen::Vector3d & v) const
{
GLint viewport[4] = {0, 0, parent()->width(), parent()->height() };
Eigen::Vector3d pos;
gluUnProject(v.x(), parent()->height() - v.y(), v.z(),
d->modelview.data(), d->projection.data(), viewport, &pos.x(), &pos.y(), &pos.z());
return pos;
}
CircularGroundPath::CircularGroundPath( Eigen::Vector3d _start_point, Eigen::Vector3d _target_point, double _angular_speed, MovementDirection _direction ):
start_point_(_start_point.x(),_start_point.y()),
end_point_(_target_point.x(),_target_point.y()),
angular_speed_(_angular_speed),
direction_(_direction)
{
}
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);
}
//! Calculate the geodetic latitude from Cartesian position and offset of z-intercept.
double calculateGeodeticLatitude( const Eigen::Vector3d cartesianState,
const double zInterceptOffset )
{
// Calculate Cartesian coordinates, Montenbruck and Gill (2000), Eq. (5.88b).
return std::atan2( cartesianState.z( ) +
zInterceptOffset, std::sqrt(
cartesianState.x( ) * cartesianState.x( ) +
cartesianState.y( ) * cartesianState.y( ) ) );
}
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;
}
//! Calculate the altitude over an oblate spheroid of a position vector from auxiliary variables.
double calculateAltitudeOverOblateSpheroid( const Eigen::Vector3d cartesianState,
const double zInterceptOffset,
const double interceptToSurfaceDistance )
{
// Calculate Cartesian coordinates, Montenbruck and Gill (2000), Eq. (5.88c).
return std::sqrt( cartesianState.x( ) * cartesianState.x( ) +
cartesianState.y( ) * cartesianState.y( ) +
( cartesianState.z( ) + zInterceptOffset ) *
( cartesianState.z( ) + zInterceptOffset ) ) - interceptToSurfaceDistance;
}
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());
}
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;
}
// 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));
}
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
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 WallBallWidget::updateUsers(){
bool userFlags[NUM_USERS];
for (int i = 0; i < NUM_USERS; ++i)
userFlags[i] = false;
AppUserMap::iterator uit;
// for each user
for(uit = users_.begin();uit!=users_.end();uit++){
// will return the existing ball if user already exists for it
GOBall* ball = WallBall::newBall(uit->first);
AppUser appUser = uit->second;
Eigen::Vector3d torso = appUser.jtPosByName("torso");
Eigen::Vector3d lShoulder = appUser.jtPosByName("left_shoulder");
Eigen::Vector3d rShoulder = appUser.jtPosByName("right_shoulder");
Eigen::Vector3d head = appUser.jtPosByName("head");
Eigen::Vector3d lHand = appUser.jtPosByName("left_hand");
Eigen::Vector3d rHand = appUser.jtPosByName("right_hand");
Eigen::Vector3d lElbow = appUser.jtPosByName("left_elbow");
Eigen::Vector3d rElbow = appUser.jtPosByName("right_elbow");
double leanOffset =lElbow.x() - lHand.x();
//double leanOffset = abs(torso.x() - rHand.x()) - abs(torso.x() - lHand.x());
//double leanOffset = torso.x() - (lShoulder.x() + rShoulder.x())/2.0f;
/*if (abs(leanOffset) < 25)
leanOffset = 0.0f;
else if (abs(leanOffset) < 45)
leanOffset = leanOffset / abs(leanOffset) * 30.0f;
else
leanOffset = leanOffset / abs(leanOffset) * 50.0f;*/
InputManager::SetMoveDirection(uit->first, leanOffset/500.0f);
//bool jump = lHand.y() > head.y() && rHand.y() > head.y();
bool jump = rHand.y() > rShoulder.y();
InputManager::SetJump(uit->first, jump);
userFlags[uit->first] = true;
}
for (int i = 0; i < NUM_USERS; ++i) {
// if a user left, remove their ball from the game
if (userFlags[i] == false && lastUserFlags[i] == true) {
WallBall::removeBall(i);
}
lastUserFlags[i] = userFlags[i];
}
}
calin::ix::iact_data::instrument_layout::ArrayLayout*
calin::simulation::vs_optics::dc_parameters_to_array_layout(
const ix::simulation::vs_optics::IsotropicDCArrayParameters& param,
calin::ix::iact_data::instrument_layout::ArrayLayout* d)
{
if(d == nullptr)d = new calin::ix::iact_data::instrument_layout::ArrayLayout;
d->set_array_type(calin::ix::iact_data::instrument_layout::ArrayLayout::NO_ARRAY);
*d->mutable_array_origin() = param.array_origin();
std::vector<Eigen::Vector3d> scope_pos;
if(param.array_layout_case() ==
IsotropicDCArrayParameters::kHexArrayLayout)
{
const auto& layout = param.hex_array_layout();
double spacing = layout.scope_spacing();
bool array_parity = layout.scope_labeling_parity();
unsigned num_telescopes =
math::hex_array::ringid_to_nsites_contained(layout.num_scope_rings());
std::set<unsigned> scopes_missing;
for(auto hexid : layout.scope_missing_list())
scopes_missing.insert(hexid);
for(unsigned hexid=0; hexid<num_telescopes; hexid++)
if(scopes_missing.find(hexid) == scopes_missing.end())
{
Eigen::Vector3d pos;
math::hex_array::hexid_to_xy(hexid, pos.x(), pos.y());
if(array_parity)pos.x() = -pos.x();
pos.x() = pos.x() * spacing;
pos.y() = pos.y() * spacing;
scope_pos.push_back(pos);
}
}
else if(param.array_layout_case() ==
IsotropicDCArrayParameters::kPrescribedArrayLayout)
{
for(auto pos : param.prescribed_array_layout().scope_positions())
scope_pos.emplace_back(calin::math::vector3d_util::from_proto(pos));
}
else
{
assert(0);
}
for(unsigned i=0; i<scope_pos.size(); i++)
dc_parameters_to_telescope_layout(param, i, scope_pos[i], d->add_telescopes());
return d;
}
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;
}
void Viewer::setCameraPosition ( const Eigen::Vector3d& position, const Eigen::Vector3d& orientation )
{
Eigen::Vector3d p = position - *m_offset;
camera()->setUpVector( QVector3D( 0, 0, m_z_up ? 1 : -1 ) );
camera()->setEye( QVector3D( p.x(), p.y(), p.z() ) );
double cos_yaw = std::cos( orientation.z() );
double sin_yaw = std::sin( orientation.z() );
double sin_pitch = std::sin( orientation.y() );
double cos_pitch = std::cos( orientation.y() );
Eigen::Vector3d direction( cos_pitch * cos_yaw, cos_pitch * sin_yaw, sin_pitch ); // todo: quick and dirty, forget about roll for now
Eigen::Vector3d scene_center = p + direction * 50; // quick and dirty
Eigen::Vector3d where = p + direction;
camera()->setCenter( QVector3D( where.x(), where.y(), where.z() ) );
m_sceneCenter = QVector3D( scene_center.x(), scene_center.y(), scene_center.z() );
}
