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);
}
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())
{
}
void OrientationTask::orientation(const Eigen::Quaterniond& ori)
{
ori_ = ori.matrix();
}