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_++; }