void ViewController::render(const Eigen::Affine3f& parentTrans)
{
Eigen::Affine3f trans = mCamera * parentTrans;
// camera position, position + size
Eigen::Vector3f viewStart = trans.inverse().translation();
Eigen::Vector3f viewEnd = trans.inverse() * Eigen::Vector3f((float)Renderer::getScreenWidth(), (float)Renderer::getScreenHeight(), 0);
// draw systemview
getSystemListView()->render(trans);
// draw gamelists
for(auto it = mGameListViews.begin(); it != mGameListViews.end(); it++)
{
// clipping
Eigen::Vector3f guiStart = it->second->getPosition();
Eigen::Vector3f guiEnd = it->second->getPosition() + Eigen::Vector3f(it->second->getSize().x(), it->second->getSize().y(), 0);
if(guiEnd.x() >= viewStart.x() && guiEnd.y() >= viewStart.y() &&
guiStart.x() <= viewEnd.x() && guiStart.y() <= viewEnd.y())
it->second->render(trans);
}
if(mWindow->peekGui() == this)
mWindow->renderHelpPromptsEarly();
// fade out
if(mFadeOpacity)
{
Renderer::setMatrix(parentTrans);
Renderer::drawRect(0, 0, Renderer::getScreenWidth(), Renderer::getScreenHeight(), 0x00000000 | (unsigned char)(mFadeOpacity * 255));
}
}
transformation objectModelSV::modelToScene( const int modelPointIndex, const Eigen::Affine3f & transformSceneToGlobal, const float alpha)
{
Eigen::Vector3f modelPoint=modelCloud->points[modelPointIndex].getVector3fMap();
Eigen::Vector3f modelNormal=modelCloud->points[modelPointIndex].getNormalVector3fMap ();
// Get transformation from model local frame to scene local frame
Eigen::Affine3f completeTransform = transformSceneToGlobal.inverse () * Eigen::AngleAxisf(alpha, Eigen::Vector3f::UnitX ()) * modelCloudTransformations[modelPointIndex];
Eigen::Quaternion<float> rotationQ=Eigen::Quaternion<float>(completeTransform.rotation());
// if object is symmetric remove yaw rotation (assume symmetry around z axis)
if(symmetric)
{
Eigen::Vector3f eulerAngles;
// primeiro [0] -> rot. around x (roll) [1] -> rot. around y (pitch) [2] -> rot. around z (yaw)
quaternionToEuler(rotationQ, eulerAngles[0], eulerAngles[1], eulerAngles[2]);
//pcl::getEulerAngles(completeTransform,eulerAngles[0], eulerAngles[1], eulerAngles[2]);
//eulerAngles[2]=0.0;
eulerToQuaternion(rotationQ, eulerAngles[0], eulerAngles[1], eulerAngles[2]);
//quaternionToEuler(rotationQ, eulerAngles[2], eulerAngles[1], eulerAngles[2]);
//std::cout << "EULER ANGLES: " << eulerAngles << std::endl;
}
//std::cout << "translation: " << completeTransform.rotation().matrix() << std::endl;
return transformation(rotationQ, Eigen::Translation3f(completeTransform.translation()) );
}
bool FootstepGraph::isGoal(StatePtr state)
{
FootstepState::Ptr goal = getGoal(state->getLeg());
if (publish_progress_) {
jsk_footstep_msgs::FootstepArray msg;
msg.header.frame_id = "odom"; // TODO fixed frame_id
msg.header.stamp = ros::Time::now();
msg.footsteps.push_back(*state->toROSMsg());
pub_progress_.publish(msg);
}
Eigen::Affine3f pose = state->getPose();
Eigen::Affine3f goal_pose = goal->getPose();
Eigen::Affine3f transformation = pose.inverse() * goal_pose;
if ((parameters_.goal_pos_thr > transformation.translation().norm()) &&
(parameters_.goal_rot_thr > std::abs(Eigen::AngleAxisf(transformation.rotation()).angle()))) {
// check collision
if (state->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
if (right_goal_state_->crossCheck(state)) {
return true;
}
} else if (state->getLeg() == jsk_footstep_msgs::Footstep::RIGHT) {
if (left_goal_state_->crossCheck(state)) {
return true;
}
}
}
return false;
}
template<typename PointT> void
pcl16::approximatePolygon (const PlanarPolygon<PointT>& polygon, PlanarPolygon<PointT>& approx_polygon, float threshold, bool refine, bool closed)
{
const Eigen::Vector4f& coefficients = polygon.getCoefficients ();
const typename pcl16::PointCloud<PointT>::VectorType &contour = polygon.getContour ();
Eigen::Vector3f rotation_axis (coefficients[1], -coefficients[0], 0.0f);
rotation_axis.normalize ();
float rotation_angle = acosf (coefficients [2]);
Eigen::Affine3f transformation = Eigen::Translation3f (0, 0, coefficients [3]) * Eigen::AngleAxisf (rotation_angle, rotation_axis);
typename pcl16::PointCloud<PointT>::VectorType polygon2D (contour.size ());
for (unsigned pIdx = 0; pIdx < polygon2D.size (); ++pIdx)
polygon2D [pIdx].getVector3fMap () = transformation * contour [pIdx].getVector3fMap ();
typename pcl16::PointCloud<PointT>::VectorType approx_polygon2D;
approximatePolygon2D<PointT> (polygon2D, approx_polygon2D, threshold, refine, closed);
typename pcl16::PointCloud<PointT>::VectorType &approx_contour = approx_polygon.getContour ();
approx_contour.resize (approx_polygon2D.size ());
Eigen::Affine3f inv_transformation = transformation.inverse ();
for (unsigned pIdx = 0; pIdx < approx_polygon2D.size (); ++pIdx)
approx_contour [pIdx].getVector3fMap () = inv_transformation * approx_polygon2D [pIdx].getVector3fMap ();
}
void
Cylinder::computePose(Eigen::Vector3f origin, Eigen::Vector3f z_axis)
{
Eigen::Affine3f t;
pcl::getTransformationFromTwoUnitVectorsAndOrigin(sym_axis_, z_axis, origin, t);
pose_ = t.inverse();
}
void
pcl::CropBox<PointT>::applyFilter (PointCloud &output)
{
output.resize (input_->points.size ());
int indice_count = 0;
// We filter out invalid points
output.is_dense = true;
Eigen::Affine3f transform = Eigen::Affine3f::Identity();
Eigen::Affine3f inverse_transform = Eigen::Affine3f::Identity();
if (rotation_ != Eigen::Vector3f::Zero ())
{
pcl::getTransformation (0, 0, 0,
rotation_ (0), rotation_ (1), rotation_ (2),
transform);
inverse_transform = transform.inverse ();
}
for (size_t index = 0; index < indices_->size (); ++index)
{
if (!input_->is_dense)
// Check if the point is invalid
if (!isFinite (input_->points[index]))
continue;
// Get local point
PointT local_pt = input_->points[(*indices_)[index]];
// Transform point to world space
if (!(transform_.matrix ().isIdentity ()))
local_pt = pcl::transformPoint<PointT> (local_pt, transform_);
if (translation_ != Eigen::Vector3f::Zero ())
{
local_pt.x -= translation_ (0);
local_pt.y -= translation_ (1);
local_pt.z -= translation_ (2);
}
// Transform point to local space of crop box
if (!(inverse_transform.matrix ().isIdentity ()))
local_pt = pcl::transformPoint<PointT> (local_pt, inverse_transform);
if (local_pt.x < min_pt_[0] || local_pt.y < min_pt_[1] || local_pt.z < min_pt_[2])
continue;
if (local_pt.x > max_pt_[0] || local_pt.y > max_pt_[1] || local_pt.z > max_pt_[2])
continue;
output.points[indice_count++] = input_->points[(*indices_)[index]];
}
output.width = indice_count;
output.height = 1;
output.resize (output.width * output.height);
}
void MsgToPoint3D(const TPPLPoint &pt, const cob_3d_mapping_msgs::Shape::ConstPtr& new_message, Eigen::Vector3f &pos, Eigen::Vector3f &normal) {
if(new_message->params.size()==4) {
Eigen::Vector3f u,v,origin;
Eigen::Affine3f transformation;
normal(0)=new_message->params[0];
normal(1)=new_message->params[1];
normal(2)=new_message->params[2];
origin(0)=new_message->centroid.x;
origin(1)=new_message->centroid.y;
origin(2)=new_message->centroid.z;
//std::cout << "normal: " << normal << std::endl;
//std::cout << "centroid: " << origin << std::endl;
v = normal.unitOrthogonal ();
u = normal.cross (v);
pcl::getTransformationFromTwoUnitVectorsAndOrigin(v, normal, origin, transformation);
transformation=transformation.inverse();
Eigen::Vector3f p3;
p3(0)=pt.x;
p3(1)=pt.y;
p3(2)=0;
pos = transformation*p3;
}
else if(new_message->params.size()==5) {
Eigen::Vector2f v,v2,n2;
v(0)=pt.x;
v(1)=pt.y;
v2=v;
v2(0)*=v2(0);
v2(1)*=v2(1);
n2(0)=new_message->params[3];
n2(1)=new_message->params[4];
//dummy normal
normal(0)=new_message->params[0];
normal(1)=new_message->params[1];
normal(2)=new_message->params[2];
Eigen::Vector3f x,y, origin;
x(0)=1.f;
y(1)=1.f;
x(1)=x(2)=y(0)=y(2)=0.f;
Eigen::Matrix<float,3,2> proj2plane_;
proj2plane_.col(0)=normal.cross(y);
proj2plane_.col(1)=normal.cross(x);
origin(0)=new_message->centroid.x;
origin(1)=new_message->centroid.y;
origin(2)=new_message->centroid.z;
pos = origin+proj2plane_*v + normal*(v2.dot(n2));
normal += normal*(v).dot(n2);
}
}
void
Cylinder::computePose(Eigen::Vector3f origin, std::vector<std::vector<Eigen::Vector3f> >& contours_3d)
{
Eigen::Affine3f t;
pcl::getTransformationFromTwoUnitVectorsAndOrigin(sym_axis_, sym_axis_.unitOrthogonal(), origin, t);
Eigen::Vector3f z_cyl = t * contours_3d[0][0];
z_cyl(1) = 0;
Eigen::Vector3f z_axis = t.inverse().rotation() * z_cyl;
computePose(origin, z_axis.normalized());
}
template<typename PointT> void
pcl::CropBox<PointT>::applyFilter (std::vector<int> &indices)
{
indices.resize (input_->points.size ());
int indice_count = 0;
Eigen::Affine3f transform = Eigen::Affine3f::Identity ();
Eigen::Affine3f inverse_transform = Eigen::Affine3f::Identity ();
if (rotation_ != Eigen::Vector3f::Zero ())
{
pcl::getTransformation (0, 0, 0,
rotation_ (0), rotation_ (1), rotation_ (2),
transform);
inverse_transform = transform.inverse ();
}
for (size_t index = 0; index < indices_->size (); ++index)
{
if (!input_->is_dense)
// Check if the point is invalid
if (!isFinite (input_->points[index]))
continue;
// Get local point
PointT local_pt = input_->points[(*indices_)[index]];
// Transform point to world space
if (!(transform_.matrix ().isIdentity ()))
local_pt = pcl::transformPoint<PointT> (local_pt, transform_);
if (translation_ != Eigen::Vector3f::Zero ())
{
local_pt.x -= translation_ (0);
local_pt.y -= translation_ (1);
local_pt.z -= translation_ (2);
}
// Transform point to local space of crop box
if (!(inverse_transform.matrix ().isIdentity ()))
local_pt = pcl::transformPoint<PointT> (local_pt, inverse_transform);
if (local_pt.x < min_pt_[0] || local_pt.y < min_pt_[1] || local_pt.z < min_pt_[2])
continue;
if (local_pt.x > max_pt_[0] || local_pt.y > max_pt_[1] || local_pt.z > max_pt_[2])
continue;
indices[indice_count++] = (*indices_)[index];
}
indices.resize (indice_count);
}
bool FootstepGraph::isGoal(StatePtr state)
{
FootstepState::Ptr goal = getGoal(state->getLeg());
if (publish_progress_) {
jsk_footstep_msgs::FootstepArray msg;
msg.header.frame_id = "odom";
msg.header.stamp = ros::Time::now();
msg.footsteps.push_back(*state->toROSMsg());
pub_progress_.publish(msg);
}
Eigen::Affine3f pose = state->getPose();
Eigen::Affine3f goal_pose = goal->getPose();
Eigen::Affine3f transformation = pose.inverse() * goal_pose;
return (pos_goal_thr_ > transformation.translation().norm()) &&
(rot_goal_thr_ > std::abs(Eigen::AngleAxisf(transformation.rotation()).angle()));
}
bool FootstepGraph::isCollidingBox(const Eigen::Affine3f& c, pcl::PointIndices::Ptr candidates) const
{
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr input_cloud = obstacle_tree_model_->getInputCloud();
Eigen::Affine3f inv_c = c.inverse();
for (size_t i = 0; i < candidates->indices.size(); i++) {
int index = candidates->indices[i];
const pcl::PointXYZ candidate_point = input_cloud->points[index];
// convert candidate_point into `c' local representation.
const Eigen::Vector3f local_p = inv_c * candidate_point.getVector3fMap();
if (std::abs(local_p[0]) < collision_bbox_size_[0] / 2 &&
std::abs(local_p[1]) < collision_bbox_size_[1] / 2 &&
std::abs(local_p[2]) < collision_bbox_size_[2] / 2) {
return true;
}
}
return false;
}
void setTrackerTarget(){
initTracking();
Eigen::Vector4f c;
Eigen::Affine3f trans = Eigen::Affine3f::Identity ();
pcl::compute3DCentroid<PointT>(*object_to_track,c);
trans.translation().matrix() = Eigen::Vector3f(c[0],c[1],c[2]);
tracker_->setTrans (trans);
pcl::PointCloud<PointT>::Ptr tmp_pc(new pcl::PointCloud<PointT>);
pcl::transformPointCloud<PointT> (*object_to_track, *tmp_pc, trans.inverse());
tracker_->setReferenceCloud(tmp_pc);
tracker_->setInputCloud(cloud);
tracked_object_centroid->clear();
tracked_object_centroid->push_back(PointT(c[0],c[1],c[2]));
}
transformation objectModelSV::modelToScene(const pcl::PointNormal & pointModel, const Eigen::Affine3f & transformSceneToGlobal, const float alpha)
{
Eigen::Vector3f modelPoint=pointModel.getVector3fMap();
Eigen::Vector3f modelNormal=pointModel.getNormalVector3fMap ();
// Get transformation from model local frame to global frame
Eigen::Vector3f cross=modelNormal.cross (Eigen::Vector3f::UnitX ()).normalized ();
Eigen::AngleAxisf rotationModelToGlobal(acosf (modelNormal.dot (Eigen::Vector3f::UnitX ())), cross);
if (isnan(cross[0]))
{
rotationModelToGlobal=Eigen::AngleAxisf(0.0,Eigen::Vector3f::UnitX ());
}
//std::cout<< "ola:" <<acosf (modelNormal.dot (Eigen::Vector3f::UnitX ()))<<std::endl;
//std::cout <<"X:"<< Eigen::Translation3f( rotationModelToGlobal * ((-1) * modelPoint)).x() << std::endl;
//std::cout <<"Y:"<< Eigen::Translation3f( rotationModelToGlobal * ((-1) * modelPoint)).y() << std::endl;
//std::cout <<"Z:"<< Eigen::Translation3f( rotationModelToGlobal * ((-1) * modelPoint)).z() << std::endl;
Eigen::Affine3f transformModelToGlobal = Eigen::Translation3f( rotationModelToGlobal * ((-1) * modelPoint)) * rotationModelToGlobal;
// Get transformation from model local frame to scene local frame
Eigen::Affine3f completeTransform = transformSceneToGlobal.inverse () * Eigen::AngleAxisf(alpha, Eigen::Vector3f::UnitX ()) * transformModelToGlobal;
//std::cout << Eigen::AngleAxisf(alpha, Eigen::Vector3f::UnitX ()).matrix() << std::endl;
Eigen::Quaternion<float> rotationQ=Eigen::Quaternion<float>(completeTransform.rotation());
// if object is symmetric remove yaw rotation (assume symmetry around z axis)
if(symmetric)
{
Eigen::Vector3f eulerAngles;
// primeiro [0] -> rot. around x (roll) [1] -> rot. around y (pitch) [2] -> rot. around z (yaw)
quaternionToEuler(rotationQ, eulerAngles[0], eulerAngles[1], eulerAngles[2]);
//pcl::getEulerAngles(completeTransform,eulerAngles[0], eulerAngles[1], eulerAngles[2]);
//eulerAngles[2]=0.0;
eulerToQuaternion(rotationQ, eulerAngles[0], eulerAngles[1], eulerAngles[2]);
//quaternionToEuler(rotationQ, eulerAngles[2], eulerAngles[1], eulerAngles[2]);
//std::cout << "EULER ANGLES: " << eulerAngles << std::endl;
}
//std::cout << "rotation: " << rotationQ << std::endl;
return transformation(rotationQ, Eigen::Translation3f(completeTransform.translation()) );
}
double
radiusAndOriginFromCloud(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr in_cloud ,
std::vector<int>& indices,
Eigen::Vector3f& origin,
const Eigen::Vector3f& sym_axis)
{
// Transform into cylinder coordinate frame
Eigen::Affine3f t;
pcl::getTransformationFromTwoUnitVectorsAndOrigin(sym_axis.unitOrthogonal(), sym_axis, origin, t);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc_trans( new pcl::PointCloud<pcl::PointXYZRGB>() );
pcl::transformPointCloud(*in_cloud, indices, *pc_trans, t);
// Inliers of circle model
pcl::PointIndices inliers;
// Coefficients of circle model
pcl::ModelCoefficients coeff;
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
// Optimize coefficients
seg.setOptimizeCoefficients (true);
// Set type of method
//seg.setMethodType (pcl::SAC_MLESAC);
seg.setMethodType (pcl::SAC_RANSAC);
// Set number of maximum iterations
seg.setMaxIterations (10);
// Set type of model
seg.setModelType (pcl::SACMODEL_CIRCLE2D);
// Set threshold of model
seg.setDistanceThreshold (0.010);
// Give as input the filtered point cloud
seg.setInputCloud (pc_trans/*in_cloud*/);
//boost::shared_ptr<std::vector<int> > indices_ptr(&indices);
//seg.setIndices(indices_ptr);
// Call the segmenting method
seg.segment(inliers, coeff);
// origin in cylinder coordinates
Eigen::Vector3f l_origin;
l_origin << coeff.values[0],coeff.values[1],0;
origin = t.inverse() * l_origin;
return coeff.values[2];
}
void SLPointCloudWidget::updateCalibration(){
CalibrationData calibration;
bool load_result = calibration.load("calibration.xml");
if (!load_result)
return;
// Camera coordinate system
visualizer->addCoordinateSystem(50, "camera", 0);
// Projector coordinate system
cv::Mat TransformPCV(4, 4, CV_32F, 0.0);
cv::Mat(calibration.Rp).copyTo(TransformPCV.colRange(0, 3).rowRange(0, 3));
cv::Mat(calibration.Tp).copyTo(TransformPCV.col(3).rowRange(0, 3));
TransformPCV.at<float>(3, 3) = 1.0; // make it homogeneous
Eigen::Affine3f TransformP;
cv::cv2eigen(TransformPCV, TransformP.matrix());
visualizer->addCoordinateSystem(50, TransformP.inverse(), "projector", 0);
}
bool normalizeSize(std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &points, std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &normals, Eigen::Affine3f &invTransform)
{
// Assumes normalized rotation/translation
Eigen::AlignedBox3f aabb;
for (size_t i = 0; i < points.size(); ++i) {
aabb.extend(points[i]);
}
// Calculate isotropic scaling to that the longest side becomes unit length
const float s = 1.f / aabb.diagonal().maxCoeff();
for (size_t i = 0; i < points.size(); ++i) {
points[i] *= s;
}
// Assemble inverse transform.
invTransform = Eigen::Affine3f::Identity();
invTransform = invTransform.scale(s);
invTransform = invTransform.inverse();
return true;
}
void
ParticleFilterTracking::resetTrackingTargetModel(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &new_target_cloud)
{
//prepare the model of tracker's target
Eigen::Vector4f c;
Eigen::Affine3f trans = Eigen::Affine3f::Identity ();
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr transed_ref (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr transed_ref_downsampled (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::compute3DCentroid (*new_target_cloud, c);
trans.translation ().matrix () = Eigen::Vector3f (c[0], c[1], c[2]);
pcl::transformPointCloud(*new_target_cloud, *transed_ref, trans.inverse());
gridSampleApprox (transed_ref, *transed_ref_downsampled, downsampling_grid_size_);
//set reference model and trans
{
boost::mutex::scoped_lock lock(mtx_);
tracker_->setReferenceCloud (transed_ref_downsampled);
tracker_->setTrans (trans);
tracker_->resetTracking();
}
//Reset target Model
ROS_INFO("RESET TARGET MODEL");
}
bool normalizeOrientationAndTranslation(std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &points, std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &normals, Eigen::Affine3f &invTransform)
{
if (points.empty())
return false;
// Perform PCA on input to determine a canoncial coordinate frame for the given point cloud.
Eigen::Matrix3Xf::MapType pointsInMatrix(points.at(0).data(), 3, static_cast<int>(points.size()));
const Eigen::Vector3f centroid = pointsInMatrix.rowwise().mean();
pointsInMatrix = pointsInMatrix.colwise() - centroid;
const Eigen::Matrix3f cov = pointsInMatrix * pointsInMatrix.transpose();
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eig(cov);
const Eigen::Matrix3f rot = eig.eigenvectors().transpose();
for (size_t i = 0; i < points.size(); ++i) {
points[i] = rot * points[i];
normals[i] = rot * normals[i];
}
invTransform = Eigen::Affine3f::Identity();
invTransform = invTransform.rotate(rot).translate(-centroid); // applied in right to left order.
invTransform = invTransform.inverse();
return true;
}
void IterativeClosestPoint::execute() {
float error = std::numeric_limits<float>::max(), previousError;
uint iterations = 0;
// Get access to the two point sets
PointSetAccess::pointer accessFixedSet = ((PointSet::pointer)getStaticInputData<PointSet>(0))->getAccess(ACCESS_READ);
PointSetAccess::pointer accessMovingSet = ((PointSet::pointer)getStaticInputData<PointSet>(1))->getAccess(ACCESS_READ);
// Get transformations of point sets
AffineTransformation::pointer fixedPointTransform2 = SceneGraph::getAffineTransformationFromData(getStaticInputData<PointSet>(0));
Eigen::Affine3f fixedPointTransform;
fixedPointTransform.matrix() = fixedPointTransform2->matrix();
AffineTransformation::pointer initialMovingTransform2 = SceneGraph::getAffineTransformationFromData(getStaticInputData<PointSet>(1));
Eigen::Affine3f initialMovingTransform;
initialMovingTransform.matrix() = initialMovingTransform2->matrix();
// These matrices are Nx3
MatrixXf fixedPoints = accessFixedSet->getPointSetAsMatrix();
MatrixXf movingPoints = accessMovingSet->getPointSetAsMatrix();
Eigen::Affine3f currentTransformation = Eigen::Affine3f::Identity();
// Want to choose the smallest one as moving
bool invertTransform = false;
if(false && fixedPoints.cols() < movingPoints.cols()) {
reportInfo() << "switching fixed and moving" << Reporter::end;
// Switch fixed and moving
MatrixXf temp = fixedPoints;
fixedPoints = movingPoints;
movingPoints = temp;
invertTransform = true;
// Apply initial transformations
//currentTransformation = fixedPointTransform.getTransform();
movingPoints = fixedPointTransform*movingPoints.colwise().homogeneous();
fixedPoints = initialMovingTransform*fixedPoints.colwise().homogeneous();
} else {
// Apply initial transformations
//currentTransformation = initialMovingTransform.getTransform();
movingPoints = initialMovingTransform*movingPoints.colwise().homogeneous();
fixedPoints = fixedPointTransform*fixedPoints.colwise().homogeneous();
}
do {
previousError = error;
MatrixXf movedPoints = currentTransformation*(movingPoints.colwise().homogeneous());
// Match closest points using current transformation
MatrixXf rearrangedFixedPoints = rearrangeMatrixToClosestPoints(
fixedPoints, movedPoints);
// Get centroids
Vector3f centroidFixed = getCentroid(rearrangedFixedPoints);
//reportInfo() << "Centroid fixed: " << Reporter::end;
//reportInfo() << centroidFixed << Reporter::end;
Vector3f centroidMoving = getCentroid(movedPoints);
//reportInfo() << "Centroid moving: " << Reporter::end;
//reportInfo() << centroidMoving << Reporter::end;
Eigen::Transform<float, 3, Eigen::Affine> updateTransform = Eigen::Transform<float, 3, Eigen::Affine>::Identity();
if(mTransformationType == IterativeClosestPoint::RIGID) {
// Create correlation matrix H of the deviations from centroid
MatrixXf H = (movedPoints.colwise() - centroidMoving)*
(rearrangedFixedPoints.colwise() - centroidFixed).transpose();
// Do SVD on H
Eigen::JacobiSVD<Eigen::MatrixXf> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
// Estimate rotation as R=V*U.transpose()
MatrixXf temp = svd.matrixV()*svd.matrixU().transpose();
Matrix3f d = Matrix3f::Identity();
d(2,2) = sign(temp.determinant());
Matrix3f R = svd.matrixV()*d*svd.matrixU().transpose();
// Estimate translation
Vector3f T = centroidFixed - R*centroidMoving;
updateTransform.linear() = R;
updateTransform.translation() = T;
} else {
// Only translation
Vector3f T = centroidFixed - centroidMoving;
updateTransform.translation() = T;
}
// Update current transformation
currentTransformation = updateTransform*currentTransformation;
// Calculate RMS error
MatrixXf distance = rearrangedFixedPoints - currentTransformation*(movingPoints.colwise().homogeneous());
error = 0;
for(uint i = 0; i < distance.cols(); i++) {
error += pow(distance.col(i).norm(),2);
}
error = sqrt(error / distance.cols());
iterations++;
reportInfo() << "Error: " << error << Reporter::end;
// To continue, change in error has to be above min error change and nr of iterations less than max iterations
} while(previousError-error > mMinErrorChange && iterations < mMaxIterations);
if(invertTransform){
currentTransformation = currentTransformation.inverse();
}
mError = error;
mTransformation->matrix() = currentTransformation.matrix();
}
int
main (int argc, char** argv)
{
if (argc < 3)
{
PCL_WARN("Please set device_id pcd_filename(e.g. $ %s '#1' sample.pcd)\n", argv[0]);
exit (1);
}
//read pcd file
target_cloud.reset(new Cloud());
if(pcl::io::loadPCDFile (argv[2], *target_cloud) == -1){
std::cout << "pcd file not found" << std::endl;
exit(-1);
}
std::string device_id = std::string (argv[1]);
counter = 0;
//Set parameters
new_cloud_ = false;
downsampling_grid_size_ = 0.002;
std::vector<double> default_step_covariance = std::vector<double> (6, 0.015 * 0.015);
default_step_covariance[3] *= 40.0;
default_step_covariance[4] *= 40.0;
default_step_covariance[5] *= 40.0;
std::vector<double> initial_noise_covariance = std::vector<double> (6, 0.00001);
std::vector<double> default_initial_mean = std::vector<double> (6, 0.0);
KLDAdaptiveParticleFilterOMPTracker<RefPointType, ParticleT>::Ptr tracker
(new KLDAdaptiveParticleFilterOMPTracker<RefPointType, ParticleT> (8));
ParticleT bin_size;
bin_size.x = 0.1f;
bin_size.y = 0.1f;
bin_size.z = 0.1f;
bin_size.roll = 0.1f;
bin_size.pitch = 0.1f;
bin_size.yaw = 0.1f;
//Set all parameters for KLDAdaptiveParticleFilterOMPTracker
tracker->setMaximumParticleNum (1000);
tracker->setDelta (0.99);
tracker->setEpsilon (0.2);
tracker->setBinSize (bin_size);
//Set all parameters for ParticleFilter
tracker_ = tracker;
tracker_->setTrans (Eigen::Affine3f::Identity ());
tracker_->setStepNoiseCovariance (default_step_covariance);
tracker_->setInitialNoiseCovariance (initial_noise_covariance);
tracker_->setInitialNoiseMean (default_initial_mean);
tracker_->setIterationNum (1);
tracker_->setParticleNum (600);
tracker_->setResampleLikelihoodThr(0.00);
tracker_->setUseNormal (false);
//Setup coherence object for tracking
ApproxNearestPairPointCloudCoherence<RefPointType>::Ptr coherence
(new ApproxNearestPairPointCloudCoherence<RefPointType>);
DistanceCoherence<RefPointType>::Ptr distance_coherence
(new DistanceCoherence<RefPointType>);
coherence->addPointCoherence (distance_coherence);
pcl::search::Octree<RefPointType>::Ptr search (new pcl::search::Octree<RefPointType> (0.01));
coherence->setSearchMethod (search);
coherence->setMaximumDistance (0.01);
tracker_->setCloudCoherence (coherence);
//prepare the model of tracker's target
Eigen::Vector4f c;
Eigen::Affine3f trans = Eigen::Affine3f::Identity ();
CloudPtr transed_ref (new Cloud);
CloudPtr transed_ref_downsampled (new Cloud);
pcl::compute3DCentroid<RefPointType> (*target_cloud, c);
trans.translation ().matrix () = Eigen::Vector3f (c[0], c[1], c[2]);
pcl::transformPointCloud<RefPointType> (*target_cloud, *transed_ref, trans.inverse());
gridSampleApprox (transed_ref, *transed_ref_downsampled, downsampling_grid_size_);
//set reference model and trans
tracker_->setReferenceCloud (transed_ref_downsampled);
tracker_->setTrans (trans);
//Setup OpenNIGrabber and viewer
pcl::visualization::CloudViewer* viewer_ = new pcl::visualization::CloudViewer("PCL OpenNI Tracking Viewer");
pcl::Grabber* interface = new pcl::OpenNIGrabber (device_id);
boost::function<void (const CloudConstPtr&)> f =
boost::bind (&cloud_cb, _1);
interface->registerCallback (f);
viewer_->runOnVisualizationThread (boost::bind(&viz_cb, _1), "viz_cb");
//Start viewer and object tracking
interface->start();
while (!viewer_->wasStopped ())
std::this_thread::sleep_for(1s);
interface->stop();
}
void
cloud_cb (const sensor_msgs::LaserScan::ConstPtr& scan)
{
sensor_msgs::PointCloud2 point_cloud2;
RefCloudPtr raw_cloud(new RefCloud);
projector_.transformLaserScanToPointCloud("base_laser_link", *scan, point_cloud2, tfListener_);
pcl17::fromROSMsg<PointType> (point_cloud2, *raw_cloud);
boost::mutex::scoped_lock lock (mtx_);
cloud_pass_.reset (new Cloud);
cloud_pass_downsampled_.reset (new Cloud);
pcl17::ModelCoefficients::Ptr coefficients (new pcl17::ModelCoefficients ());
pcl17::PointIndices::Ptr inliers (new pcl17::PointIndices ());
filterPassThrough (raw_cloud, *cloud_pass_);
if (counter_ < 10)
{
gridSample (cloud_pass_, *cloud_pass_downsampled_, downsampling_grid_size_);
}
else if (counter_ == 10)
{
//gridSample (cloud_pass_, *cloud_pass_downsampled_, 0.01);
cloud_pass_downsampled_ = cloud_pass_;
CloudPtr target_cloud;
if (target_cloud != NULL)
{
RefCloudPtr nonzero_ref (new RefCloud);
removeZeroPoints (input_model_point_cloud_, *nonzero_ref);
// PCL_INFO ("calculating cog\n");
Eigen::Vector4f c;
RefCloudPtr transed_ref (new RefCloud);
pcl17::compute3DCentroid<RefPointType> (*nonzero_ref, c);
Eigen::Affine3f trans = Eigen::Affine3f::Identity ();
trans.translation ().matrix () = Eigen::Vector3f (c[0], c[1], c[2]);
pcl17::transformPointCloud<RefPointType> (*nonzero_ref, *transed_ref, trans.inverse());
CloudPtr transed_ref_downsampled (new Cloud);
gridSample (transed_ref, *transed_ref_downsampled, downsampling_grid_size_);
tracker_->setReferenceCloud (transed_ref_downsampled);
tracker_->setTrans (trans);
tracker_->setMinIndices (int (input_model_point_cloud_->points.size ()) / 2);
}
else
{
// PCL_WARN ("euclidean segmentation failed\n");
}
}
else
{
gridSampleApprox (cloud_pass_, *cloud_pass_downsampled_, downsampling_grid_size_);
tracking (cloud_pass_downsampled_);
}
new_cloud_ = true;
counter_++;
}
int
main (int argc, char** argv)
{
// ros::init(argc, argv, "extract_sec");
// ros::NodeHandle node_handle;
// pcl::visualization::PCLVisualizer result_viewer("planar_segmentation");
boost::shared_ptr<pcl::visualization::PCLVisualizer> result_viewer (new pcl::visualization::PCLVisualizer ("planar_segmentation"));
result_viewer->addCoordinateSystem(0.3, "reference", 0);
result_viewer->setCameraPosition(-0.499437, 0.111597, -0.758007, -0.443141, 0.0788583, -0.502855, -0.034703, -0.992209, -0.119654);
result_viewer->setCameraClipDistances(0.739005, 2.81526);
// result_viewer->setCameraPosition(Position, Focal point, View up);
// result_viewer->setCameraClipDistances(Clipping plane);
/***************************************
* parse arguments
***************************************/
if(argc<5)
{
pcl::console::print_info("Usage: extract_sec DATA_PATH/PCD_FILE_FORMAT START_INDEX END_INDEX DEMO_NAME (opt)STEP_SIZE(1)");
exit(1);
}
int view_id=0;
int step=1;
std::string basename_cloud=argv[1];
unsigned int index_start = std::atoi(argv[2]);
unsigned int index_end = std::atoi(argv[3]);
std::string demo_name=argv[4];
if(argc>5) step=std::atoi(argv[5]);
/***************************************
* set up result directory
***************************************/
mkdir("/home/zhen/Documents/Dataset/human_result", S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
char result_folder[50];
std::snprintf(result_folder, sizeof(result_folder), "/home/zhen/Documents/Dataset/human_result/%s", demo_name.c_str());
mkdir(result_folder, S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
std::string basename_pcd = (basename_cloud.find(".pcd") == std::string::npos) ? (basename_cloud + ".pcd") : basename_cloud;
std::string filename_pcd;
std::string mainGraph_file;
mainGraph_file = std::string(result_folder) + "/mainGraph.txt";
// write video config
char video_file[100];
std::snprintf(video_file, sizeof(video_file), "%s/video.txt", result_folder);
std::ofstream video_config(video_file);
if (video_config.is_open())
{
video_config << index_start << " " << index_end << " " << demo_name << " " << step;
video_config.close();
}
/***************************************
* set up cloud, segmentation, tracker, detectors, graph, features
***************************************/
TableObject::Segmentation tableObjSeg;
TableObject::Segmentation initialSeg;
TableObject::track3D tracker(false);
TableObject::colorDetector finger1Detector(0,100,0,100,100,200); //right
TableObject::colorDetector finger2Detector(150,250,0,100,0,100); //left
TableObject::touchDetector touchDetector(0.01);
TableObject::bottleDetector bottleDetector;
TableObject::mainGraph mainGraph((int)index_start);
std::vector<manipulation_features> record_features;
manipulation_features cur_features;
TableObject::pcdCloud pcdSceneCloud;
CloudPtr sceneCloud;
CloudPtr planeCloud(new Cloud);
CloudPtr cloud_objects(new Cloud);
CloudPtr cloud_finger1(new Cloud);
CloudPtr cloud_finger2(new Cloud);
CloudPtr cloud_hull(new Cloud);
CloudPtr track_target(new Cloud);
CloudPtr tracked_cloud(new Cloud);
std::vector<pcl::PointIndices> clusters;
pcl::ModelCoefficients coefficients;
pcl::PointIndices f1_indices;
pcl::PointIndices f2_indices;
Eigen::Affine3f toBottleCoordinate;
Eigen::Affine3f transformation;
Eigen::Vector3f bottle_init_ori;
// set threshold of size of clustered cloud
tableObjSeg.setThreshold(30);
initialSeg.setThreshold(500);
// downsampler
pcl::ApproximateVoxelGrid<pcl::PointXYZRGBA> grid;
float leaf_size=0.005;
grid.setLeafSize (leaf_size, leaf_size, leaf_size);
/***************************************
* start processing
***************************************/
unsigned int idx = index_start;
int video_id=0;
bool change = false;
while( idx <= index_end && !result_viewer->wasStopped())
{
std::cout << std::endl;
std::cout << "frame id=" << idx << std::endl;
filename_pcd = cv::format(basename_cloud.c_str(), idx);
if(idx==index_start) {
/***************************************
* Intialization:
* -plane localization
* -object cluster extraction
* -bottle cluster localization
***************************************/
initialSeg.resetCloud(filename_pcd, false);
initialSeg.seg(false);
initialSeg.getObjects(cloud_objects, clusters);
initialSeg.getCloudHull(cloud_hull);
initialSeg.getPlaneCoefficients(coefficients);
initialSeg.getsceneCloud(pcdSceneCloud);
initialSeg.getTableTopCloud(planeCloud);
sceneCloud=pcdSceneCloud.getCloud();
/***************************************
* fingertip, hand_arm removal
***************************************/
//opencv color filtering for fingertip_1
{
pcl::ScopeTime t_finger1("Finger 1(blue) detection");
finger1Detector.setInputCloud(cloud_objects, clusters);
finger1Detector.filter(f1_indices,cloud_finger1);
}
finger1Detector.showDetectedCloud(result_viewer, "finger1");
//opencv color filtering for fingertip_2
{
pcl::ScopeTime t_finger2("Finger 2(orange) detection");
finger2Detector.setInputCloud(cloud_objects, clusters);
finger2Detector.filter(f2_indices,cloud_finger2);
}
finger2Detector.showDetectedCloud(result_viewer, "finger2");
// remove hand (include cluster that contains the detected fingertips and also the other clusters that are touching the cluster)
std::vector<int> hand_arm1=TableObject::findHand(cloud_objects, clusters, f1_indices);
for(int i=hand_arm1.size()-1; i>=0; i--)
{
clusters.erase(clusters.begin()+hand_arm1[i]);
std::cout << "removing hand_arm : cluster index = " << hand_arm1[i] << std::endl;
}
std::vector<int> hand_arm2=TableObject::findHand(cloud_objects, clusters, f2_indices);
for(int i=hand_arm2.size()-1; i>=0; i--)
{
clusters.erase(clusters.begin()+hand_arm2[i]);
std::cout << "removing hand_arm : cluster index = " << hand_arm2[i] << std::endl;
}
// DEBUG
// pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBA> plane(planeCloud);
// result_viewer->addPointCloud<RefPointType>(planeCloud, plane, "tabletop");
// CloudPtr debug(new Cloud);
// initialSeg.getOutPlaneCloud(debug);
// pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBA> out_plane(debug);
// result_viewer->addPointCloud<RefPointType>(debug, out_plane, "out_plane");
// choose bottle_id at 1st frame & confirm fitted model is correct
TableObject::view3D::drawClusters(result_viewer, cloud_objects, clusters, true);
while (!result_viewer->wasStopped ())
{
result_viewer->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
std::cout << "cluster size = " << clusters.size() << std::endl;
/***************************************
* Localizing cylinder
***************************************/
CloudPtr cluster_bottle (new Cloud);
int bottle_id = 0;
pcl::copyPointCloud (*cloud_objects, clusters[bottle_id], *cluster_bottle);
bottleDetector.setInputCloud(cluster_bottle);
bottleDetector.fit();
bottleDetector.getTransformation(toBottleCoordinate);
bottle_init_ori= bottleDetector.getOrientation();
float x, y, z, roll, pitch, yaw;
pcl::getTranslationAndEulerAngles(toBottleCoordinate.inverse(), x, y, z, roll, pitch, yaw);
result_viewer->removeCoordinateSystem("reference", 0);
result_viewer->addCoordinateSystem(0.3, toBottleCoordinate.inverse(), "reference", 0);
bottleDetector.drawOrientation(result_viewer);
/***************************************
* Record features
***************************************/
bottle_features cur_bottle_features;
cur_bottle_features.loc[0] = x;
cur_bottle_features.loc[1] = y;
cur_bottle_features.loc[2] = z;
cur_bottle_features.ori[0] = roll;
cur_bottle_features.ori[1] = pitch;
cur_bottle_features.ori[2] = yaw;
cur_bottle_features.color[0] = bottleDetector.getCenter().r;
cur_bottle_features.color[1] = bottleDetector.getCenter().g;
cur_bottle_features.color[2] = bottleDetector.getCenter().b;
cur_bottle_features.size[0] = bottleDetector.getHeight();
cur_bottle_features.size[1] = bottleDetector.getRadius();
cur_features.bottle = cur_bottle_features;
pcl::PointXYZ center_finger1 = TableObject::computeObjCentroid(cloud_finger1);
pcl::PointXYZ center_finger2 = TableObject::computeObjCentroid(cloud_finger2);
center_finger1 = pcl::transformPoint<pcl::PointXYZ>(center_finger1, toBottleCoordinate);
center_finger2 = pcl::transformPoint<pcl::PointXYZ>(center_finger2, toBottleCoordinate);
cur_features.gripper_1.loc[0] = center_finger1.x;
cur_features.gripper_1.loc[1] = center_finger1.y;
cur_features.gripper_1.loc[2] = center_finger1.z;
cur_features.gripper_2.loc[0] = center_finger2.x;
cur_features.gripper_2.loc[1] = center_finger2.y;
cur_features.gripper_2.loc[2] = center_finger2.z;
record_features.push_back(cur_features);
/***************************************
* Tracking initialization
***************************************/
{
pcl::ScopeTime t_track("Tracker initialization");
tracker.setTarget(cluster_bottle, bottleDetector.getCenter());
tracker.initialize();
}
/***************************************
* Touch detection
***************************************/
std::vector<CloudPtr> touch_clouds;
touch_clouds.push_back(cluster_bottle);
touch_clouds.push_back(cloud_finger1);
touch_clouds.push_back(cloud_finger2);
// touch detection between each pair of objects (including fingertips, tabletop objects and tabletop)
for(int i=0; i<touch_clouds.size(); i++)
{
int j;
bool touch;
for(j=i+1; j<touch_clouds.size(); j++)
{
// touch detection between object_i and object_j
char relation [50];
std::sprintf(relation, "object%d_object%d", i, j);
std::cout << relation << std::endl;
{
pcl::ScopeTime t("Touch detection");
touch=touchDetector.detect(touch_clouds[i], touch_clouds[j]);
}
// touchDetector.showTouch(result_viewer, relation, 100+250*(j-i-1), 40+20*i);
// relational scene graph -> main graph
if(touch) {
mainGraph.addInitialRelationalGraph(2);
} else {
mainGraph.addInitialRelationalGraph(0);
}
}
// touch detection between each objects and tabletop
char relation [50];
std::sprintf (relation, "object%d_object%d", i, (int)touch_clouds.size());
std::cout << relation << std::endl;
{
pcl::ScopeTime t("Touch detection");
touch=touchDetector.detectTableTouch(touch_clouds[i], coefficients);
}
// touchDetector.showTouch(result_viewer, relation, 100+250*(j-i-1), 40+20*i);
// relational scene graph -> main graph
if(touch) {
mainGraph.addInitialRelationalGraph(2);
} else {
mainGraph.addInitialRelationalGraph(0);
}
}
/***************************************
* Visualization
***************************************/
// draw extracted object clusters
// TableObject::view3D::drawClusters(result_viewer, cloud_objects, touch_clusters);
// draw extracted plane points
// pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBA> plane(planeCloud);
// result_viewer->addPointCloud<RefPointType>(planeCloud, plane, "tabletop");
// std::stringstream ss;
// ss << (int)touch_clusters.size();
// result_viewer->addText3D(ss.str(), planeCloud->points.at(334*640+78),0.1);
// draw extracted plane contour polygon
result_viewer->addPolygon<RefPointType>(cloud_hull, 0, 255, 0, "polygon");
change = true;
} else
{
/***************************************
* object cloud extraction
***************************************/
tableObjSeg.resetCloud(filename_pcd, false);
tableObjSeg.seg(cloud_hull,false);
tableObjSeg.getObjects(cloud_objects, clusters);
tableObjSeg.getsceneCloud(pcdSceneCloud);
sceneCloud=pcdSceneCloud.getCloud();
/***************************************
* fingertip extraction
***************************************/
//opencv color filtering for fingertip_1
{
pcl::ScopeTime t_finger1("Finger 1(blue) detection");
finger1Detector.setInputCloud(cloud_objects, clusters);
finger1Detector.filter(f1_indices,cloud_finger1);
}
finger1Detector.showDetectedCloud(result_viewer, "finger1");
//opencv color filtering for fingertip_2
{
pcl::ScopeTime t_finger1("Finger 2(orange) detection");
finger2Detector.setInputCloud(cloud_objects, clusters);
finger2Detector.filter(f2_indices,cloud_finger2);
}
finger2Detector.showDetectedCloud(result_viewer, "finger2");
/***************************************
* filter out black glove cluster & gray sleeve, also update cloud_objects with removed cluster indices
***************************************/
for(int i=0; i<clusters.size(); i++)
{
pcl::CentroidPoint<RefPointType> color_points;
for(int j=0; j<clusters[i].indices.size(); j++)
{
color_points.add(cloud_objects->at(clusters[i].indices[j]));
}
RefPointType mean_color;
color_points.get(mean_color);
if(mean_color.r>30 & mean_color.r<70 & mean_color.g>30 & mean_color.g<70 & mean_color.b>30 & mean_color.b<70)
{
clusters.erase(clusters.begin()+ i);
i=i-1;
}
}
/***************************************
* Tracking objects
***************************************/
{
pcl::ScopeTime t_track("Tracking");
grid.setInputCloud (sceneCloud);
grid.filter (*track_target);
tracker.track(track_target, transformation);
tracker.getTrackedCloud(tracked_cloud);
}
tracker.viewTrackedCloud(result_viewer);
// tracker.drawParticles(result_viewer);
/***************************************
* compute tracked <center, orientation>
***************************************/
pcl::PointXYZ bottle_loc_point(0,0,0);
bottle_loc_point = pcl::transformPoint<pcl::PointXYZ>(bottle_loc_point, transformation);
result_viewer->removeShape("bottle_center");
// result_viewer->addSphere<pcl::PointXYZ>(bottle_loc_point, 0.05, "bottle_center");
Eigen::Vector3f bottle_ori;
pcl::transformVector(bottle_init_ori,bottle_ori,transformation);
TableObject::view3D::drawArrow(result_viewer, bottle_loc_point, bottle_ori, "bottle_arrow");
/***************************************
* calculate toTrackedBottleCoordinate
***************************************/
Eigen::Affine3f toTrackedBottleCoordinate;
Eigen::Vector3f p( bottle_loc_point.x, bottle_loc_point.y, bottle_loc_point.z ); // position
// get a vector that is orthogonal to _orientation ( yc = _orientation x [1,0,0]' )
Eigen::Vector3f yc( 0, bottle_ori[2], -bottle_ori[1] );
yc.normalize();
// get a transform that rotates _orientation into z and moves cloud into origin.
pcl::getTransformationFromTwoUnitVectorsAndOrigin(yc, bottle_ori, p, toTrackedBottleCoordinate);
result_viewer->removeCoordinateSystem("reference");
result_viewer->addCoordinateSystem(0.3, toTrackedBottleCoordinate.inverse(), "reference", 0);
float x, y, z, roll, pitch, yaw;
pcl::getTranslationAndEulerAngles(toTrackedBottleCoordinate.inverse(), x, y, z, roll, pitch, yaw);
/***************************************
* setup bottle feature
***************************************/
cur_features = record_features[video_id-1];
cur_features.bottle.loc[0] = x;
cur_features.bottle.loc[1] = y;
cur_features.bottle.loc[2] = z;
cur_features.bottle.ori[0] = roll;
cur_features.bottle.ori[1] = pitch;
cur_features.bottle.ori[2] = yaw;
pcl::PointXYZ center_finger1 = TableObject::computeObjCentroid(cloud_finger1);
pcl::PointXYZ center_finger2 = TableObject::computeObjCentroid(cloud_finger2);
center_finger1 = pcl::transformPoint<pcl::PointXYZ>(center_finger1, toTrackedBottleCoordinate);
center_finger2 = pcl::transformPoint<pcl::PointXYZ>(center_finger2, toTrackedBottleCoordinate);
cur_features.gripper_1.loc[0] = center_finger1.x;
cur_features.gripper_1.loc[1] = center_finger1.y;
cur_features.gripper_1.loc[2] = center_finger1.z;
cur_features.gripper_2.loc[0] = center_finger2.x;
cur_features.gripper_2.loc[1] = center_finger2.y;
cur_features.gripper_2.loc[2] = center_finger2.z;
record_features.push_back(cur_features);
/***************************************
* Touch detection
***************************************/
std::vector<CloudPtr> touch_clouds;
touch_clouds.push_back(tracked_cloud);
touch_clouds.push_back(cloud_finger1);
touch_clouds.push_back(cloud_finger2);
// touch detection between each pair of objects (including fingertips, tabletop objects and tabletop)
for(int i=0; i<touch_clouds.size(); i++)
{
int j;
bool touch;
for(j=i+1; j<touch_clouds.size(); j++)
{
// touch detection between object_i and object_j
char relation [50];
std::sprintf(relation, "object%d_object%d", i, j);
std::cout << relation << std::endl;
{
pcl::ScopeTime t("Touch detection");
touch=touchDetector.detect(touch_clouds[i], touch_clouds[j]);
}
// touchDetector.showTouch(result_viewer, relation, 100+250*(j-i-1), 40+20*i);
// relational scene graph -> main graph
if(touch) {
mainGraph.addRelationalGraph(2);
} else {
mainGraph.addRelationalGraph(0);
}
}
// touch detection between each objects and tabletop
char relation [50];
std::sprintf (relation, "object%d_object%d", i, (int)touch_clouds.size());
std::cout << relation << std::endl;
{
pcl::ScopeTime t("Touch detection");
touch=touchDetector.detectTableTouch(touch_clouds[i], coefficients);
}
// touchDetector.showTouch(result_viewer, relation, 100+250*(j-i-1), 40+20*i);
// relational scene graph -> main graph
if(touch) {
mainGraph.addRelationalGraph(2);
} else {
mainGraph.addRelationalGraph(0);
}
}
/***************************************
* Visualization
***************************************/
// draw extracted point clusters
// TableObject::view3D::drawText(result_viewer, cloud_objects, touch_clusters);
/***************************************
* Main Graph
***************************************/
change = mainGraph.compareRelationGraph((int)idx);
}
// darw original cloud
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBA> rgb(sceneCloud);
if(!result_viewer->updatePointCloud<RefPointType>(sceneCloud, rgb, "new frame"))
result_viewer->addPointCloud<RefPointType>(sceneCloud, rgb, "new frame");
result_viewer->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
//debug
std::cout << cur_features.bottle.loc[0] << " " << cur_features.bottle.loc[1] << " " << cur_features.bottle.loc[2]
<< " " << cur_features.bottle.ori[0] << " " << cur_features.bottle.ori[1] << " " << cur_features.bottle.ori[2]
<< " " << cur_features.bottle.color[0] << " " << cur_features.bottle.color[1] << " " << cur_features.bottle.color[2]
<< " " << cur_features.bottle.size[0] << " " << cur_features.bottle.size[1]
<< " " << cur_features.gripper_1.loc[0] << " " << cur_features.gripper_1.loc[1] << " " << cur_features.gripper_1.loc[2]
<< " " << cur_features.gripper_2.loc[0] << " " << cur_features.gripper_2.loc[1] << " " << cur_features.gripper_2.loc[2]
<< std::endl;
if(change)
{
char screenshot[100]; // make sure it's big enough
std::snprintf(screenshot, sizeof(screenshot), "%s/sec_%d.png", result_folder, (int)idx);
std::cout << screenshot << std::endl;
result_viewer->saveScreenshot(screenshot);
//record features
char feature_file[100]; // make sure it's big enough
std::snprintf(feature_file, sizeof(feature_file), "%s/features_original.txt", result_folder);
std::ofstream feature_writer(feature_file, std::ofstream::out | std::ofstream::app);
feature_writer << cur_features.bottle.loc[0] << " " << cur_features.bottle.loc[1] << " " << cur_features.bottle.loc[2]
<< " " << cur_features.bottle.ori[0] << " " << cur_features.bottle.ori[1] << " " << cur_features.bottle.ori[2]
<< " " << cur_features.bottle.color[0] << " " << cur_features.bottle.color[1] << " " << cur_features.bottle.color[2]
<< " " << cur_features.bottle.size[0] << " " << cur_features.bottle.size[1]
<< " " << cur_features.gripper_1.loc[0] << " " << cur_features.gripper_1.loc[1] << " " << cur_features.gripper_1.loc[2]
<< " " << cur_features.gripper_2.loc[0] << " " << cur_features.gripper_2.loc[1] << " " << cur_features.gripper_2.loc[2]
<< std::endl;
feature_writer.close();
std::cout << "features saved at " << feature_file << std::endl;
}
char screenshot[200]; // make sure it's big enough
std::snprintf(screenshot, sizeof(screenshot), "%s/video/sec_%d.png", result_folder, (int)video_id);
std::cout << screenshot << std::endl;
result_viewer->saveScreenshot(screenshot);
idx=idx+step;
video_id=video_id+1;
}
mainGraph.displayMainGraph();
mainGraph.recordMainGraph(mainGraph_file);
while (!result_viewer->wasStopped ())
{
result_viewer->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
}
void
pcl::kinfuLS::WorldModel<PointT>::getExistingData(const double previous_origin_x, const double previous_origin_y, const double previous_origin_z, const double offset_x, const double offset_y, const double offset_z, const double volume_x, const double volume_y, const double volume_z, pcl::PointCloud<PointT> &existing_slice)
{
double newOriginX = previous_origin_x + offset_x;
double newOriginY = previous_origin_y + offset_y;
double newOriginZ = previous_origin_z + offset_z;
double newLimitX = newOriginX + volume_x;
double newLimitY = newOriginY + volume_y;
double newLimitZ = newOriginZ + volume_z;
// filter points in the space of the new cube
PointCloudPtr newCube (new pcl::PointCloud<PointT>);
// condition
ConditionAndPtr range_condAND (new pcl::ConditionAnd<PointT> ());
range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, newOriginX)));
range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, newLimitX)));
range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, newOriginY)));
range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, newLimitY)));
range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, newOriginZ)));
range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, newLimitZ)));
// build the filter
pcl::ConditionalRemoval<PointT> condremAND (true);
condremAND.setCondition (range_condAND);
condremAND.setInputCloud (world_);
condremAND.setKeepOrganized (false);
// apply filter
condremAND.filter (*newCube);
// filter points that belong to the new slice
ConditionOrPtr range_condOR (new pcl::ConditionOr<PointT> ());
if(offset_x >= 0)
range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, previous_origin_x + volume_x - 1.0 )));
else
range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, previous_origin_x )));
if(offset_y >= 0)
range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, previous_origin_y + volume_y - 1.0 )));
else
range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, previous_origin_y )));
if(offset_z >= 0)
range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, previous_origin_z + volume_z - 1.0 )));
else
range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, previous_origin_z )));
// build the filter
pcl::ConditionalRemoval<PointT> condrem (true);
condrem.setCondition (range_condOR);
condrem.setInputCloud (newCube);
condrem.setKeepOrganized (false);
// apply filter
condrem.filter (existing_slice);
if(existing_slice.points.size () != 0)
{
//transform the slice in new cube coordinates
Eigen::Affine3f transformation;
transformation.translation ()[0] = newOriginX;
transformation.translation ()[1] = newOriginY;
transformation.translation ()[2] = newOriginZ;
transformation.linear ().setIdentity ();
transformPointCloud (existing_slice, existing_slice, transformation.inverse ());
}
}
void
pcl::CropBox<pcl::PCLPointCloud2>::applyFilter (std::vector<int> &indices)
{
indices.resize (input_->width * input_->height);
removed_indices_->resize (input_->width * input_->height);
int indices_count = 0;
int removed_indices_count = 0;
Eigen::Affine3f transform = Eigen::Affine3f::Identity();
Eigen::Affine3f inverse_transform = Eigen::Affine3f::Identity();
if (rotation_ != Eigen::Vector3f::Zero ())
{
pcl::getTransformation (0, 0, 0,
rotation_ (0), rotation_ (1), rotation_ (2),
transform);
inverse_transform = transform.inverse();
}
//PointXYZ local_pt;
Eigen::Vector3f local_pt (Eigen::Vector3f::Zero ());
for (size_t index = 0; index < indices_->size (); index++)
{
// Get local point
int point_offset = ((*indices_)[index] * input_->point_step);
int offset = point_offset + input_->fields[x_idx_].offset;
memcpy (&local_pt, &input_->data[offset], sizeof (float)*3);
// Transform point to world space
if (!(transform_.matrix().isIdentity()))
local_pt = transform_ * local_pt;
if (translation_ != Eigen::Vector3f::Zero ())
{
local_pt.x () -= translation_ (0);
local_pt.y () -= translation_ (1);
local_pt.z () -= translation_ (2);
}
// Transform point to local space of crop box
if (!(inverse_transform.matrix().isIdentity()))
local_pt = inverse_transform * local_pt;
// If outside the cropbox
if ( (local_pt.x () < min_pt_[0] || local_pt.y () < min_pt_[1] || local_pt.z () < min_pt_[2]) ||
(local_pt.x () > max_pt_[0] || local_pt.y () > max_pt_[1] || local_pt.z () > max_pt_[2]))
{
if (negative_)
{
indices[indices_count++] = (*indices_)[index];
}
else if (extract_removed_indices_)
{
(*removed_indices_)[removed_indices_count++] = static_cast<int> (index);
}
}
// If inside the cropbox
else
{
if (negative_ && extract_removed_indices_)
{
(*removed_indices_)[removed_indices_count++] = static_cast<int> (index);
}
else if (!negative_) {
indices[indices_count++] = (*indices_)[index];
}
}
}
indices.resize (indices_count);
removed_indices_->resize (removed_indices_count);
}
void
pcl::CropBox<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
{
// Resize output cloud to sample size
output.data.resize (input_->data.size ());
removed_indices_->resize (input_->data.size ());
// Copy the common fields
output.fields = input_->fields;
output.is_bigendian = input_->is_bigendian;
output.row_step = input_->row_step;
output.point_step = input_->point_step;
output.height = 1;
int indices_count = 0;
int removed_indices_count = 0;
Eigen::Affine3f transform = Eigen::Affine3f::Identity();
Eigen::Affine3f inverse_transform = Eigen::Affine3f::Identity();
if (rotation_ != Eigen::Vector3f::Zero ())
{
pcl::getTransformation (0, 0, 0,
rotation_ (0), rotation_ (1), rotation_ (2),
transform);
inverse_transform = transform.inverse();
}
//PointXYZ local_pt;
Eigen::Vector3f local_pt (Eigen::Vector3f::Zero ());
for (size_t index = 0; index < indices_->size (); ++index)
{
// Get local point
int point_offset = ((*indices_)[index] * input_->point_step);
int offset = point_offset + input_->fields[x_idx_].offset;
memcpy (&local_pt, &input_->data[offset], sizeof (float)*3);
// Check if the point is invalid
if (!pcl_isfinite (local_pt.x ()) ||
!pcl_isfinite (local_pt.y ()) ||
!pcl_isfinite (local_pt.z ()))
continue;
// Transform point to world space
if (!(transform_.matrix().isIdentity()))
local_pt = transform_ * local_pt;
if (translation_ != Eigen::Vector3f::Zero ())
{
local_pt.x () = local_pt.x () - translation_ (0);
local_pt.y () = local_pt.y () - translation_ (1);
local_pt.z () = local_pt.z () - translation_ (2);
}
// Transform point to local space of crop box
if (!(inverse_transform.matrix ().isIdentity ()))
local_pt = inverse_transform * local_pt;
// If outside the cropbox
if ( (local_pt.x () < min_pt_[0] || local_pt.y () < min_pt_[1] || local_pt.z () < min_pt_[2]) ||
(local_pt.x () > max_pt_[0] || local_pt.y () > max_pt_[1] || local_pt.z () > max_pt_[2]))
{
if (negative_)
{
memcpy (&output.data[indices_count++ * output.point_step],
&input_->data[index * output.point_step], output.point_step);
}
else if (extract_removed_indices_)
{
(*removed_indices_)[removed_indices_count++] = static_cast<int> (index);
}
}
// If inside the cropbox
else
{
if (negative_ && extract_removed_indices_)
{
(*removed_indices_)[removed_indices_count++] = static_cast<int> (index);
}
else if (!negative_) {
memcpy (&output.data[indices_count++ * output.point_step],
&input_->data[index * output.point_step], output.point_step);
}
}
}
output.width = indices_count;
output.row_step = output.point_step * output.width;
output.data.resize (output.width * output.height * output.point_step);
removed_indices_->resize (removed_indices_count);
}
template<typename Point> void
TableObjectCluster<Point>::calculateBoundingBox(
const PointCloudPtr& cloud,
const pcl::PointIndices& indices,
const Eigen::Vector3f& plane_normal,
const Eigen::Vector3f& plane_point,
Eigen::Vector3f& position,
Eigen::Quaternion<float>& orientation,
Eigen::Vector3f& size)
{
// transform to table coordinate frame and project points on X-Y, save max height
Eigen::Affine3f tf;
pcl::getTransformationFromTwoUnitVectorsAndOrigin(plane_normal.unitOrthogonal(), plane_normal, plane_point, tf);
pcl::PointCloud<pcl::PointXYZ>::Ptr pc2d(new pcl::PointCloud<pcl::PointXYZ>);
float height = 0.0;
for(std::vector<int>::const_iterator it=indices.indices.begin(); it != indices.indices.end(); ++it)
{
Eigen::Vector3f tmp = tf * (*cloud)[*it].getVector3fMap();
height = std::max<float>(height, fabs(tmp(2)));
pc2d->push_back(pcl::PointXYZ(tmp(0),tmp(1),0.0));
}
// create convex hull of projected points
#ifdef PCL_MINOR_VERSION >= 6
pcl::PointCloud<pcl::PointXYZ>::Ptr conv_hull(new pcl::PointCloud<pcl::PointXYZ>);
pcl::ConvexHull<pcl::PointXYZ> chull;
chull.setDimension(2);
chull.setInputCloud(pc2d);
chull.reconstruct(*conv_hull);
#endif
/*for(int i=0; i<conv_hull->size(); ++i)
std::cout << (*conv_hull)[i].x << "," << (*conv_hull)[i].y << std::endl;*/
// find the minimal bounding rectangle in 2D and table coordinates
Eigen::Vector2f p1, p2, p3;
cob_3d_mapping::MinimalRectangle2D mr2d;
mr2d.setConvexHull(conv_hull->points);
mr2d.rotatingCalipers(p2, p1, p3);
/*std::cout << "BB: \n" << p1[0] << "," << p1[1] <<"\n"
<< p2[0] << "," << p2[1] <<"\n"
<< p3[0] << "," << p3[1] <<"\n ---" << std::endl;*/
// compute center of rectangle
position[0] = 0.5f*(p1[0] + p3[0]);
position[1] = 0.5f*(p1[1] + p3[1]);
position[2] = 0.0f;
// transform back
Eigen::Affine3f inv_tf = tf.inverse();
position = inv_tf * position;
// set size of bounding box
float norm_1 = (p3-p2).norm();
float norm_2 = (p1-p2).norm();
// BoundingBox coordinates: X:= main direction, Z:= table normal
Eigen::Vector3f direction; // y direction
if (norm_1 < norm_2)
{
direction = Eigen::Vector3f(p3[0]-p2[0], p3[1]-p2[1], 0) / (norm_1);
size[0] = norm_2 * 0.5f;
size[1] = norm_1 * 0.5f;
}
else
{
direction = Eigen::Vector3f(p1[0]-p2[0], p1[1]-p2[1], 0) / (norm_2);
size[0] = norm_1 * 0.5f;
size[1] = norm_2 * 0.5f;
}
size[2] = -height;
direction = inv_tf.rotation() * direction;
orientation = pcl::getTransformationFromTwoUnitVectors(direction, plane_normal).rotation(); // (y, z-direction)
return;
Eigen::Matrix3f M = Eigen::Matrix3f::Identity() - plane_normal * plane_normal.transpose();
Eigen::Vector3f xn = M * Eigen::Vector3f::UnitX(); // X-axis project on normal
Eigen::Vector3f xxn = M * direction;
float cos_phi = acos(xn.normalized().dot(xxn.normalized())); // angle between xn and main direction
cos_phi = cos(0.5f * cos_phi);
float sin_phi = sqrt(1.0f-cos_phi*cos_phi);
//orientation.w() = cos_phi;
//orientation.x() = sin_phi * plane_normal(0);
//orientation.y() = sin_phi * plane_normal(1);
//orientation.z() = sin_phi * plane_normal(2);
}
void jsk_pcl_ros::DepthImageCreator::publish_points(const sensor_msgs::CameraInfoConstPtr& info,
const sensor_msgs::PointCloud2ConstPtr& pcloud2) {
JSK_ROS_DEBUG("DepthImageCreator::publish_points");
if (!pcloud2) return;
bool proc_cloud = true, proc_image = true, proc_disp = true;
if ( pub_cloud_.getNumSubscribers()==0 ) {
proc_cloud = false;
}
if ( pub_image_.getNumSubscribers()==0 ) {
proc_image = false;
}
if ( pub_disp_image_.getNumSubscribers()==0 ) {
proc_disp = false;
}
if( !proc_cloud && !proc_image && !proc_disp) return;
int width = info->width;
int height = info->height;
float fx = info->P[0];
float cx = info->P[2];
float tx = info->P[3];
float fy = info->P[5];
float cy = info->P[6];
Eigen::Affine3f sensorPose;
{
tf::StampedTransform transform;
if(use_fixed_transform) {
transform = fixed_transform;
} else {
try {
tf_listener_->waitForTransform(pcloud2->header.frame_id,
info->header.frame_id,
info->header.stamp,
ros::Duration(0.001));
tf_listener_->lookupTransform(pcloud2->header.frame_id,
info->header.frame_id,
info->header.stamp, transform);
}
catch ( std::runtime_error e ) {
JSK_ROS_ERROR("%s",e.what());
return;
}
}
tf::Vector3 p = transform.getOrigin();
tf::Quaternion q = transform.getRotation();
sensorPose = (Eigen::Affine3f)Eigen::Translation3f(p.getX(), p.getY(), p.getZ());
Eigen::Quaternion<float> rot(q.getW(), q.getX(), q.getY(), q.getZ());
sensorPose = sensorPose * rot;
if (tx != 0.0) {
Eigen::Affine3f trans = (Eigen::Affine3f)Eigen::Translation3f(-tx/fx , 0, 0);
sensorPose = sensorPose * trans;
}
#if 0 // debug print
JSK_ROS_INFO("%f %f %f %f %f %f %f %f %f, %f %f %f",
sensorPose(0,0), sensorPose(0,1), sensorPose(0,2),
sensorPose(1,0), sensorPose(1,1), sensorPose(1,2),
sensorPose(2,0), sensorPose(2,1), sensorPose(2,2),
sensorPose(0,3), sensorPose(1,3), sensorPose(2,3));
#endif
}
PointCloud pointCloud;
pcl::RangeImagePlanar rangeImageP;
{
// code here is dirty, some bag is in RangeImagePlanar
PointCloud tpc;
pcl::fromROSMsg(*pcloud2, tpc);
Eigen::Affine3f inv;
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 5 )
inv = sensorPose.inverse();
pcl::transformPointCloud< Point > (tpc, pointCloud, inv);
#else
pcl::getInverse(sensorPose, inv);
pcl::getTransformedPointCloud<PointCloud> (tpc, inv, pointCloud);
#endif
Eigen::Affine3f dummytrans;
dummytrans.setIdentity();
rangeImageP.createFromPointCloudWithFixedSize (pointCloud,
width/scale_depth, height/scale_depth,
cx/scale_depth, cy/scale_depth,
fx/scale_depth, fy/scale_depth,
dummytrans); //sensorPose);
}
cv::Mat mat(rangeImageP.height, rangeImageP.width, CV_32FC1);
float *tmpf = (float *)mat.ptr();
for(unsigned int i = 0; i < rangeImageP.height*rangeImageP.width; i++) {
tmpf[i] = rangeImageP.points[i].z;
}
if(scale_depth != 1.0) {
cv::Mat tmpmat(info->height, info->width, CV_32FC1);
cv::resize(mat, tmpmat, cv::Size(info->width, info->height)); // LINEAR
//cv::resize(mat, tmpmat, cv::Size(info->width, info->height), 0.0, 0.0, cv::INTER_NEAREST);
mat = tmpmat;
}
if (proc_image) {
sensor_msgs::Image pubimg;
pubimg.header = info->header;
pubimg.width = info->width;
pubimg.height = info->height;
pubimg.encoding = "32FC1";
pubimg.step = sizeof(float)*info->width;
pubimg.data.resize(sizeof(float)*info->width*info->height);
// publish image
memcpy(&(pubimg.data[0]), mat.ptr(), sizeof(float)*info->height*info->width);
pub_image_.publish(boost::make_shared<sensor_msgs::Image>(pubimg));
}
if(proc_cloud || proc_disp) {
// publish point cloud
pcl::RangeImagePlanar rangeImagePP;
rangeImagePP.setDepthImage ((float *)mat.ptr(),
width, height,
cx, cy, fx, fy);
#if PCL_MAJOR_VERSION == 1 && PCL_MINOR_VERSION >= 7
rangeImagePP.header = pcl_conversions::toPCL(info->header);
#else
rangeImagePP.header = info->header;
#endif
if(proc_cloud) {
pub_cloud_.publish(boost::make_shared<pcl::PointCloud<pcl::PointWithRange > >
( (pcl::PointCloud<pcl::PointWithRange>)rangeImagePP) );
}
if(proc_disp) {
stereo_msgs::DisparityImage disp;
#if PCL_MAJOR_VERSION == 1 && PCL_MINOR_VERSION >= 7
disp.header = pcl_conversions::fromPCL(rangeImagePP.header);
#else
disp.header = rangeImagePP.header;
#endif
disp.image.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
disp.image.height = rangeImagePP.height;
disp.image.width = rangeImagePP.width;
disp.image.step = disp.image.width * sizeof(float);
disp.f = fx; disp.T = 0.075;
disp.min_disparity = 0;
disp.max_disparity = disp.T * disp.f / 0.3;
disp.delta_d = 0.125;
disp.image.data.resize (disp.image.height * disp.image.step);
float *data = reinterpret_cast<float*> (&disp.image.data[0]);
float normalization_factor = disp.f * disp.T;
for (int y = 0; y < (int)rangeImagePP.height; y++ ) {
for (int x = 0; x < (int)rangeImagePP.width; x++ ) {
pcl::PointWithRange p = rangeImagePP.getPoint(x,y);
data[y*disp.image.width+x] = normalization_factor / p.z;
}
}
pub_disp_image_.publish(boost::make_shared<stereo_msgs::DisparityImage> (disp));
}
}
}
void keypoint_map::align_z_axis() {
pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud(
new pcl::PointCloud<pcl::PointXYZ>);
for (int v = 0; v < depth_imgs[0].rows; v++) {
for (int u = 0; u < depth_imgs[0].cols; u++) {
if (depth_imgs[0].at<unsigned short>(v, u) != 0) {
pcl::PointXYZ p;
p.z = depth_imgs[0].at<unsigned short>(v, u) / 1000.0f;
p.x = (u - intrinsics[2]) * p.z / intrinsics[0];
p.y = (v - intrinsics[3]) * p.z / intrinsics[1];
p.getVector4fMap() = camera_positions[0] * p.getVector4fMap();
point_cloud->push_back(p);
}
}
}
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
// Optional
seg.setOptimizeCoefficients(true);
// Mandatory
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold(0.005);
seg.setProbability(0.99);
seg.setInputCloud(point_cloud);
seg.segment(*inliers, *coefficients);
std::cerr << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " " << coefficients->values[2] << " "
<< coefficients->values[3] << " Num inliers "
<< inliers->indices.size() << std::endl;
Eigen::Affine3f transform = Eigen::Affine3f::Identity();
if (coefficients->values[2] > 0) {
transform.matrix().coeffRef(0, 2) = coefficients->values[0];
transform.matrix().coeffRef(1, 2) = coefficients->values[1];
transform.matrix().coeffRef(2, 2) = coefficients->values[2];
} else {
transform.matrix().coeffRef(0, 2) = -coefficients->values[0];
transform.matrix().coeffRef(1, 2) = -coefficients->values[1];
transform.matrix().coeffRef(2, 2) = -coefficients->values[2];
}
transform.matrix().col(0).head<3>() =
transform.matrix().col(1).head<3>().cross(
transform.matrix().col(2).head<3>());
transform.matrix().col(1).head<3>() =
transform.matrix().col(2).head<3>().cross(
transform.matrix().col(0).head<3>());
transform = transform.inverse();
transform.matrix().coeffRef(2, 3) = coefficients->values[3];
pcl::transformPointCloud(keypoints3d, keypoints3d, transform);
for (size_t i = 0; i < camera_positions.size(); i++) {
camera_positions[i] = transform * camera_positions[i];
}
}
void keypoint_map::optimize() {
g2o::SparseOptimizer optimizer;
optimizer.setVerbose(true);
g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
linearSolver = new g2o::LinearSolverCholmod<
g2o::BlockSolver_6_3::PoseMatrixType>();
g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
g2o::OptimizationAlgorithmLevenberg* solver =
new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);
double focal_length = intrinsics[0];
Eigen::Vector2d principal_point(intrinsics[2], intrinsics[3]);
g2o::CameraParameters * cam_params = new g2o::CameraParameters(focal_length,
principal_point, 0.);
cam_params->setId(0);
if (!optimizer.addParameter(cam_params)) {
assert(false);
}
std::cerr << camera_positions.size() << " " << keypoints3d.size() << " "
<< observations.size() << std::endl;
int vertex_id = 0, point_id = 0;
for (size_t i = 0; i < camera_positions.size(); i++) {
Eigen::Affine3f cam_world = camera_positions[i].inverse();
Eigen::Vector3d trans(cam_world.translation().cast<double>());
Eigen::Quaterniond q(cam_world.rotation().cast<double>());
g2o::SE3Quat pose(q, trans);
g2o::VertexSE3Expmap * v_se3 = new g2o::VertexSE3Expmap();
v_se3->setId(vertex_id);
if (i < 1) {
v_se3->setFixed(true);
}
v_se3->setEstimate(pose);
optimizer.addVertex(v_se3);
vertex_id++;
}
for (size_t i = 0; i < keypoints3d.size(); i++) {
g2o::VertexSBAPointXYZ * v_p = new g2o::VertexSBAPointXYZ();
v_p->setId(vertex_id + point_id);
v_p->setMarginalized(true);
v_p->setEstimate(keypoints3d[i].getVector3fMap().cast<double>());
optimizer.addVertex(v_p);
point_id++;
}
for (size_t i = 0; i < observations.size(); i++) {
g2o::EdgeProjectXYZ2UV * e = new g2o::EdgeProjectXYZ2UV();
e->setVertex(0,
dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertices().find(
vertex_id + observations[i].point_id)->second));
e->setVertex(1,
dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertices().find(
observations[i].cam_id)->second));
e->setMeasurement(observations[i].coord.cast<double>());
e->information() = Eigen::Matrix2d::Identity();
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
e->setParameterId(0, 0);
optimizer.addEdge(e);
}
//optimizer.save("debug.txt");
optimizer.initializeOptimization();
optimizer.setVerbose(true);
std::cout << std::endl;
std::cout << "Performing full BA:" << std::endl;
optimizer.optimize(1);
std::cout << std::endl;
for (int i = 0; i < vertex_id; i++) {
g2o::HyperGraph::VertexIDMap::iterator v_it = optimizer.vertices().find(
i);
if (v_it == optimizer.vertices().end()) {
std::cerr << "Vertex " << i << " not in graph!" << std::endl;
exit(-1);
}
g2o::VertexSE3Expmap * v_c =
dynamic_cast<g2o::VertexSE3Expmap *>(v_it->second);
if (v_c == 0) {
std::cerr << "Vertex " << i << "is not a VertexSE3Expmap!"
<< std::endl;
exit(-1);
}
Eigen::Affine3f pos;
pos.fromPositionOrientationScale(
v_c->estimate().translation().cast<float>(),
v_c->estimate().rotation().cast<float>(),
Eigen::Vector3f(1, 1, 1));
camera_positions[i] = pos.inverse();
}
for (int i = 0; i < point_id; i++) {
g2o::HyperGraph::VertexIDMap::iterator v_it = optimizer.vertices().find(
vertex_id + i);
if (v_it == optimizer.vertices().end()) {
std::cerr << "Vertex " << vertex_id + i << " not in graph!"
<< std::endl;
exit(-1);
}
g2o::VertexSBAPointXYZ * v_p =
dynamic_cast<g2o::VertexSBAPointXYZ *>(v_it->second);
if (v_p == 0) {
std::cerr << "Vertex " << vertex_id + i
<< "is not a VertexSE3Expmap!" << std::endl;
exit(-1);
}
keypoints3d[i].getVector3fMap() = v_p->estimate().cast<float>();
}
}
void
cloud_cb (const CloudConstPtr &cloud)
{
boost::mutex::scoped_lock lock (mtx_);
double start = pcl::getTime ();
FPS_CALC_BEGIN;
cloud_pass_.reset (new Cloud);
cloud_pass_downsampled_.reset (new Cloud);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
filterPassThrough (cloud, *cloud_pass_);
if (counter_ < 10)
{
gridSample (cloud_pass_, *cloud_pass_downsampled_, downsampling_grid_size_);
}
else if (counter_ == 10)
{
//gridSample (cloud_pass_, *cloud_pass_downsampled_, 0.01);
cloud_pass_downsampled_ = cloud_pass_;
CloudPtr target_cloud;
if (use_convex_hull_)
{
planeSegmentation (cloud_pass_downsampled_, *coefficients, *inliers);
if (inliers->indices.size () > 3)
{
CloudPtr cloud_projected (new Cloud);
cloud_hull_.reset (new Cloud);
nonplane_cloud_.reset (new Cloud);
planeProjection (cloud_pass_downsampled_, *cloud_projected, coefficients);
convexHull (cloud_projected, *cloud_hull_, hull_vertices_);
extractNonPlanePoints (cloud_pass_downsampled_, cloud_hull_, *nonplane_cloud_);
target_cloud = nonplane_cloud_;
}
else
{
PCL_WARN ("cannot segment plane\n");
}
}
else
{
PCL_WARN ("without plane segmentation\n");
target_cloud = cloud_pass_downsampled_;
}
if (target_cloud != NULL)
{
PCL_INFO ("segmentation, please wait...\n");
std::vector<pcl::PointIndices> cluster_indices;
euclideanSegment (target_cloud, cluster_indices);
if (cluster_indices.size () > 0)
{
// select the cluster to track
CloudPtr temp_cloud (new Cloud);
extractSegmentCluster (target_cloud, cluster_indices, 0, *temp_cloud);
Eigen::Vector4f c;
pcl::compute3DCentroid<RefPointType> (*temp_cloud, c);
int segment_index = 0;
double segment_distance = c[0] * c[0] + c[1] * c[1];
for (size_t i = 1; i < cluster_indices.size (); i++)
{
temp_cloud.reset (new Cloud);
extractSegmentCluster (target_cloud, cluster_indices, int (i), *temp_cloud);
pcl::compute3DCentroid<RefPointType> (*temp_cloud, c);
double distance = c[0] * c[0] + c[1] * c[1];
if (distance < segment_distance)
{
segment_index = int (i);
segment_distance = distance;
}
}
segmented_cloud_.reset (new Cloud);
extractSegmentCluster (target_cloud, cluster_indices, segment_index, *segmented_cloud_);
//pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
//normalEstimation (segmented_cloud_, *normals);
RefCloudPtr ref_cloud (new RefCloud);
//addNormalToCloud (segmented_cloud_, normals, *ref_cloud);
ref_cloud = segmented_cloud_;
RefCloudPtr nonzero_ref (new RefCloud);
removeZeroPoints (ref_cloud, *nonzero_ref);
PCL_INFO ("calculating cog\n");
RefCloudPtr transed_ref (new RefCloud);
pcl::compute3DCentroid<RefPointType> (*nonzero_ref, c);
Eigen::Affine3f trans = Eigen::Affine3f::Identity ();
trans.translation ().matrix () = Eigen::Vector3f (c[0], c[1], c[2]);
//pcl::transformPointCloudWithNormals<RefPointType> (*ref_cloud, *transed_ref, trans.inverse());
pcl::transformPointCloud<RefPointType> (*nonzero_ref, *transed_ref, trans.inverse());
CloudPtr transed_ref_downsampled (new Cloud);
gridSample (transed_ref, *transed_ref_downsampled, downsampling_grid_size_);
tracker_->setReferenceCloud (transed_ref_downsampled);
tracker_->setTrans (trans);
reference_ = transed_ref;
tracker_->setMinIndices (int (ref_cloud->points.size ()) / 2);
}
else
{
PCL_WARN ("euclidean segmentation failed\n");
}
}
}
else
{
//normals_.reset (new pcl::PointCloud<pcl::Normal>);
//normalEstimation (cloud_pass_downsampled_, *normals_);
//RefCloudPtr tracking_cloud (new RefCloud ());
//addNormalToCloud (cloud_pass_downsampled_, normals_, *tracking_cloud);
//tracking_cloud = cloud_pass_downsampled_;
//*cloud_pass_downsampled_ = *cloud_pass_;
//cloud_pass_downsampled_ = cloud_pass_;
gridSampleApprox (cloud_pass_, *cloud_pass_downsampled_, downsampling_grid_size_);
tracking (cloud_pass_downsampled_);
}
new_cloud_ = true;
double end = pcl::getTime ();
computation_time_ = end - start;
FPS_CALC_END("computation");
counter_++;
}