Box(Eigen::Vector4d ¢er, Eigen::Vector4d& u, Eigen::Vector4d &v,
Eigen::Vector4d &w, unsigned int id, unsigned int matId,
double uWidth, double vWidth, double wWidth)
: QuadricCollection(center, id, matId), u(u), v(v), w(w) {
BOOST_ASSERT_MSG(u[3] == 0 && v[3] == 0 && w[3] == 0, "u,v,w must have"
" fourth coordinate equal to zero!");// Got u:\n"
// << u << "v:\n" << v << "w:\n" << w)
// Prepare rotation matrix
Eigen::Matrix4d R;
R.row(0) = u;
R.row(1) = v;
R.row(2) = w;
R.row(3) = Eigen::Vector4d(0, 0, 0, 1);
/* Make all the planes forming the box (centered at origin).
Plane normals will be unit vectors pointing in positive/negative
x, y, z directions. The points x on the plane with normal
n = (a,b,c), distance d to origin satisfy n.p -d = 0,
or x^T Q x = 0 where
Q = |0 0 0 a/2|
|0 0 0 b/2|
|0 0 0 c/2|
|a/2 b/2 c/2 -d|
We define planes w.r.t. x, y, z axes, then rotate to u,v,w
*/
Eigen::Matrix4d posWPlane, negWPlane, posUPlane, negUPlane,
posVPlane, negVPlane;
posWPlane = negWPlane = posUPlane = negUPlane = posVPlane = negVPlane
= Eigen::Matrix4d::Zero();
posUPlane.row(3) = posUPlane.col(3) = Eigen::Vector4d(0.5, 0, 0, -uWidth/2.0);
negUPlane.row(3) = negUPlane.col(3) = Eigen::Vector4d(-0.5, 0, 0, -uWidth/2.0);
posVPlane.row(3) = posVPlane.col(3) = Eigen::Vector4d(0, 0.5, 0, -vWidth/2.0);
negVPlane.row(3) = negVPlane.col(3) = Eigen::Vector4d(0, -0.5, 0, -vWidth/2.0);
posWPlane.row(3) = posWPlane.col(3) = Eigen::Vector4d(0, 0, 0.5, -wWidth/2.0);
negWPlane.row(3) = negWPlane.col(3) = Eigen::Vector4d(0, 0, -0.5, -wWidth/2.0);
addQuadric(R.transpose() * posWPlane * R);
addQuadric(R.transpose() * negWPlane * R);
addQuadric(R.transpose() * posUPlane * R);
addQuadric(R.transpose() * negUPlane * R);
addQuadric(R.transpose() * posVPlane * R);
addQuadric(R.transpose() * negVPlane * R);
}
const CPoint3DCAMERA CMiniVisionToolbox::getPointStereoLinearTriangulationSVDDLT( const cv::Point2d& p_ptPointLEFT, const cv::Point2d& p_ptPointRIGHT, const Eigen::Matrix< double, 3, 4 >& p_matProjectionLEFT, const Eigen::Matrix< double, 3, 4 >& p_matProjectionRIGHT )
{
//ds A matrix for system: A*X=0
Eigen::Matrix4d matAHomogeneous;
matAHomogeneous.row(0) = p_ptPointLEFT.x*p_matProjectionLEFT.row(2)-p_matProjectionLEFT.row(0);
matAHomogeneous.row(1) = p_ptPointLEFT.y*p_matProjectionLEFT.row(2)-p_matProjectionLEFT.row(1);
matAHomogeneous.row(2) = p_ptPointRIGHT.x*p_matProjectionRIGHT.row(2)-p_matProjectionRIGHT.row(0);
matAHomogeneous.row(3) = p_ptPointRIGHT.y*p_matProjectionRIGHT.row(2)-p_matProjectionRIGHT.row(1);
//ds solve system subject to ||A*x||=0 ||x||=1
const Eigen::JacobiSVD< Eigen::Matrix4d > matSVD( matAHomogeneous, Eigen::ComputeFullU | Eigen::ComputeFullV );
//ds solution x is the last column of V
const Eigen::Vector4d vecX( matSVD.matrixV( ).col( 3 ) );
return vecX.head( 3 )/vecX(3);
}
void Data4Viewer::drawGlobalView()
{
_pKinect->_pRGBCamera->setGLProjectionMatrix(0.1f, 100.f);
glMatrixMode(GL_MODELVIEW);
Eigen::Affine3d tmp; tmp.setIdentity();
Eigen::Matrix4d mMat;
mMat.row(0) = tmp.matrix().row(0);
mMat.row(1) = -tmp.matrix().row(1);
mMat.row(2) = -tmp.matrix().row(2);
mMat.row(3) = tmp.matrix().row(3);
glLoadMatrixd(mMat.data());
GpuMat rgb(_pKinect->_cvmRGB);
_pKinect->_pRGBCamera->renderCameraInLocal(rgb, _pGL.get(), false, NULL, 0.2f, true); //render in model coordinate
//cout<<("drawRGBView");
return;
}
Eigen::Matrix4d createHomogeneousTransformMatrix(tf::StampedTransform transform)
{
tf::Point p = transform.getOrigin();
tf::Quaternion q = transform.getRotation();
tf::Matrix3x3 R1(q);
Eigen::Matrix3d R2;
tf::matrixTFToEigen(R1, R2);
ROS_INFO_STREAM("R2:\n"<<R2);
Eigen::Matrix4d T;
T.block(0, 0, 3, 3) << R2;
T.block(0, 3, 3, 1) << p.x(), p.y(), p.z();
T.row(3) << 0, 0, 0, 1;
return T;
}
void
pcl::visualization::Camera::computeViewMatrix(Eigen::Matrix4d& view_mat) const
{
//constructs view matrix from camera pos, view up, and the point it is looking at
//this code is based off of gluLookAt http://www.opengl.org/wiki/GluLookAt_code
Eigen::Vector3d focal_point (focal[0], focal[1], focal[2]);
Eigen::Vector3d posv (pos[0] , pos[1] , pos[2]);
Eigen::Vector3d up (view[0] , view[1] , view[2]);
Eigen::Vector3d zAxis = (focal_point - posv).normalized();
Eigen::Vector3d xAxis = zAxis.cross(up).normalized();
// make sure the y-axis is orthogonal to the other two
Eigen::Vector3d yAxis = xAxis.cross (zAxis);
view_mat.block <1, 3> (0, 0) = xAxis;
view_mat.block <1, 3> (1, 0) = yAxis;
view_mat.block <1, 3> (2, 0) = -zAxis;
view_mat.row (3) << 0, 0, 0, 1;
view_mat.block <3, 1> (0, 3) = view_mat.topLeftCorner<3, 3> () * (-posv);
}
int main(int argc, char** argv)
{
// initialize ROS
ros::init(argc, argv, "find_grasps");
ros::NodeHandle node("~");
GraspLocalizer::Parameters params;
// camera transforms (poses)
Eigen::Matrix4d base_tf, sqrt_tf;
base_tf << 0, 0.445417, 0.895323, 0.215,
1, 0, 0, -0.015,
0, 0.895323, -0.445417, 0.23,
0, 0, 0, 1;
sqrt_tf << 0.9366, -0.0162, 0.3500, -0.2863,
0.0151, 0.9999, 0.0058, 0.0058,
-0.3501, -0.0002, 0.9367, 0.0554,
0, 0, 0, 1;
params.cam_tf_left_ = base_tf * sqrt_tf.inverse();
params.cam_tf_right_ = base_tf * sqrt_tf;
// read ROS parameters
std::string cloud_topic;
std::string cloud_frame;
std::string svm_file_name;
std::vector<double> workspace;
std::vector<double> camera_pose;
int cloud_type;
bool parallel;
node.param("parallel", parallel, true);
node.param("cloud_topic", cloud_topic, CLOUD_TOPIC);
node.param("cloud_frame", cloud_frame, CLOUD_FRAME);
node.param("cloud_type", cloud_type, CLOUD_TYPE);
node.param("svm_file_name", svm_file_name, SVM_FILE_NAME);
node.param("num_threads", params.num_threads_, NUM_THREADS);
node.param("num_samples", params.num_samples_, NUM_SAMPLES);
node.param("num_clouds", params.num_clouds_, NUM_CLOUDS);
if (parallel) {
double finger_width, hand_outer_diameter, hand_depth;
node.param("finger_width", finger_width, FINGER_WIDTH);
node.param("hand_outer_diameter", hand_outer_diameter, HAND_OUTER_DIAMETER);
node.param("hand_depth", hand_depth, HAND_DEPTH);
params.finger_hand_ = new ParallelHand(finger_width, hand_outer_diameter, hand_depth);
}
//TODO else
node.param("init_bite", params.init_bite_, INIT_BITE);
node.param("hand_height", params.hand_height_, HAND_HEIGHT);
node.param("min_inliers", params.min_inliers_, MIN_HANDLE_INLIERS);
node.getParam("workspace", workspace);
node.getParam("camera_pose", camera_pose);
node.param("plotting", params.plotting_mode_, 0);
node.param("marker_lifetime", params.marker_lifetime_, 0.0);
Eigen::Matrix4d R;
for (int i=0; i < R.rows(); i++)
R.row(i) << camera_pose[i*R.cols()], camera_pose[i*R.cols() + 1], camera_pose[i*R.cols() + 2], camera_pose[i*R.cols() + 3];
Eigen::VectorXd ws(6);
ws << workspace[0], workspace[1], workspace[2], workspace[3], workspace[4], workspace[5];
params.workspace_ = ws;
std::cout << "-- Parameters --\n";
std::cout << " Input\n";
std::cout << " cloud_topic: " << cloud_topic << "\n";
std::cout << " cloud_frame: " << cloud_frame << "\n";
std::cout << " cloud_type: " << CLOUD_TYPES[cloud_type] << "\n";
std::cout << " Hand Search\n";
std::cout << " workspace: " << ws.transpose() << "\n";
std::cout << " num_samples: " << params.num_samples_ << "\n";
std::cout << " num_threads: " << params.num_threads_ << "\n";
std::cout << " num_clouds: " << params.num_clouds_ << "\n";
std::cout << " camera pose:\n" << R << std::endl;
std::cout << " Robot Hand Model\n";
//TODO: Make FingerGrasp printable.
// std::cout << " finger_width: " << params.finger_width_ << "\n";
// std::cout << " hand_outer_diameter: " << params.hand_outer_diameter_ << "\n";
// std::cout << " hand_depth: " << params.finger_width_ << "\n";
// std::cout << " init_bite: " << params.finger_width_ << "\n";
// std::cout << " hand_height: " << params.finger_width_ << "\n";
std::cout << " Antipodal Grasps Prediction\n";
std::cout << " svm_file_name: " << svm_file_name << "\n";
std::cout << " Handle Search\n";
std::cout << " min_inliers: " << params.min_inliers_ << "\n";
std::cout << " Visualization\n";
std::cout << " plot_mode: " << PLOT_MODES[params.plotting_mode_] << "\n";
std::cout << " marker_lifetime: " << params.marker_lifetime_ << "\n";
GraspLocalizer loc(node, cloud_topic, cloud_frame, cloud_type, svm_file_name, params);
loc.localizeGrasps();
return 0;
}