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;
  }
Esempio n. 4
0
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();
 }
Esempio n. 6
0
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());
  }
Esempio n. 9
0
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);
}
Esempio n. 10
0
 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()));
 }
Esempio n. 11
0
 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;
 }
Esempio n. 12
0
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];
  }
Esempio n. 15
0
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;
}
Esempio n. 19
0
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();
}
Esempio n. 20
0
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_++;
  }
Esempio n. 22
0
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));
    }

}
Esempio n. 23
0
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 ());
	
  }
}
Esempio n. 24
0
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);
}
Esempio n. 25
0
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>();
	}

}
Esempio n. 30
0
 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_++;
 }