Пример #1
0
void Camera::init(const mo3d::NVM_Camera* cam, int width, int height, const int maxLevel){

	projection_.resize(maxLevel+1);
	kMat_.resize(maxLevel+1);

	// calculate projection matrix for level 0
	kMat_[0] <<  cam->f , 0 , width/2.0 , 0 , cam->f , height/2.0 , 0, 0, 1;


	Eigen::Quaterniond q;
	q.w() = cam->rq[0];
	q.x() = cam->rq[1];
	q.y() = cam->rq[2];
	q.z() = cam->rq[3];

	projection_[0].col(3) = -q.matrix().cast<float>() * cam->c.cast<float>() ;
	projection_[0].topLeftCorner(3,3) = q.matrix().cast<float>();

	projection_[0] = kMat_[0] * projection_[0];

	// now propagate to lower levels:
	for (int l = 1; l < maxLevel+1; l++){
		projection_[l] = projection_[l-1];
		projection_[l].row(0) /= 2.0;
		projection_[l].row(1) /= 2.0;

		kMat_[l] = kMat_[l-1];
		kMat_[l].row(0) /= 2.0;
		kMat_[l].row(1) /= 2.0;
	}

	// optical center
	center_.head(3) = cam->c.cast<float>();
	center_(3) = 1;

	// axis of the camera
	oAxis_ =  projection_[0].row(2);
	oAxis_ /= projection_[0].row(2).head(3).norm();

	zAxis_ = oAxis_.head(3);
	xAxis_ = projection_[0].row(0).head(3);
	yAxis_ = zAxis_.cross(xAxis_).normalized();
	xAxis_ = yAxis_.cross(zAxis_).normalized();

	// get the average pixelscale
	ipscale_ = (projection_[0].row(0).head(3).norm() + projection_[0].row(1).head(3).norm())/2.0;

}
/* Receives feedback from the interactive marker and updates the shape pose in the world accordingly */
void  MotionPlanningFrame::imProcessFeedback(visualization_msgs::InteractiveMarkerFeedback &feedback)
{
  bool oldState = ui_->object_x->blockSignals(true);
  ui_->object_x->setValue(feedback.pose.position.x);
  ui_->object_x->blockSignals(oldState);

  oldState = ui_->object_y->blockSignals(true);
  ui_->object_y->setValue(feedback.pose.position.y);
  ui_->object_y->blockSignals(oldState);

  oldState = ui_->object_z->blockSignals(true);
  ui_->object_z->setValue(feedback.pose.position.z);
  ui_->object_z->blockSignals(oldState);

  Eigen::Quaterniond q;
  tf::quaternionMsgToEigen(feedback.pose.orientation, q);
  Eigen::Vector3d xyz = q.matrix().eulerAngles(0, 1, 2);

  oldState = ui_->object_rx->blockSignals(true);
  ui_->object_rx->setValue(xyz[0]);
  ui_->object_rx->blockSignals(oldState);

  oldState = ui_->object_ry->blockSignals(true);
  ui_->object_ry->setValue(xyz[1]);
  ui_->object_ry->blockSignals(oldState);

  oldState = ui_->object_rz->blockSignals(true);
  ui_->object_rz->setValue(xyz[2]);
  ui_->object_rz->blockSignals(oldState);

  updateCollisionObjectPose(false);
}
Пример #3
0
OrientationTask::OrientationTask(const rbd::MultiBody& mb, int bodyId, const Eigen::Quaterniond& ori):
	ori_(ori.matrix()),
	bodyIndex_(mb.bodyIndexById(bodyId)),
	jac_(mb, bodyId),
	eval_(3),
	speed_(3),
	normalAcc_(3),
	jacMat_(3, mb.nrDof()),
	jacDotMat_(3, mb.nrDof())
{
}
Пример #4
0
void OrientationTask::orientation(const Eigen::Quaterniond& ori)
{
	ori_ = ori.matrix();
}