Eigen::Affine3f WorldDownloadManager::toEigenAffine(const T1 & linear,const T2 & translation)
{
Eigen::Matrix3f l;
Eigen::Vector3f t;
for (uint r = 0; r < 3; r++)
for (uint c = 0; c < 3; c++)
l(r,c) = linear[r * 3 + c];
for (uint r = 0; r < 3; r++)
t[r] = translation[r];
Eigen::Affine3f result;
result.linear() = l;
result.translation() = t;
return result;
}
void SLTrackerWorker::trackPointCloud(PointCloudConstPtr pointCloud){
// Recursively call self until latest event is hit
busy = true;
QCoreApplication::sendPostedEvents(this, QEvent::MetaCall);
bool result = busy;
busy = false;
if(!result){
std::cerr << "SLTrackerWorker: dropped point cloud!" << std::endl;
return;
}
if(!referenceSet){
tracker->setReference(pointCloud);
referenceSet = true;
return;
}
performanceTime.start();
Eigen::Affine3f T;
bool converged;
float RMS;
tracker->determineTransformation(pointCloud, T, converged, RMS);
// Emit result
if(converged)
emit newPoseEstimate(T);
// std::cout << "Pose: " << T.matrix() << std::endl;
std::cout << "Tracker: " << performanceTime.elapsed() << "ms" << std::endl;
if(writeToDisk){
Eigen::Vector3f t(T.translation());
Eigen::Quaternionf q(T.rotation());
(*ofStream) << trackingTime.elapsed() << ",";
(*ofStream) << t.x() << "," << t.y() << "," << t.z() << ",";
(*ofStream) << q.w() << "," << q.x() << "," << q.y() << "," << q.z() << "," << RMS;
(*ofStream) << std::endl;
}
}
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]));
}
void
printPose( Eigen::Affine3f const& pose )
{
// debug
std::cout << pose.linear() << std::endl <<
pose.translation().transpose() << std::endl;
float alpha = atan2( pose.linear()(1,0), pose.linear()(0,0) );
float beta = atan2( -pose.linear()(2,0),
sqrt( pose.linear()(2,1) * pose.linear()(2,1) +
pose.linear()(2,2) * pose.linear()(2,2) )
);
float gamma = atan2( pose.linear()(2,1), pose.linear()(2,2) );
std::cout << "alpha: " << alpha << " " << alpha * 180.f / M_PI << std::endl;
std::cout << "beta: " << beta << " " << beta * 180.f / M_PI << std::endl;
std::cout << "gamma: " << gamma << " " << gamma * 180.f / M_PI << std::endl;
}
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()) );
}
//Draw model reference point cloud
void
drawResult (pcl::visualization::PCLVisualizer& viz)
{
ParticleXYZRPY result = tracker_->getResult ();
Eigen::Affine3f transformation = tracker_->toEigenMatrix (result);
//move close to camera a little for better visualization
transformation.translation () += Eigen::Vector3f (0.0f, 0.0f, -0.005f);
CloudPtr result_cloud (new Cloud ());
pcl::transformPointCloud<RefPointType> (*(tracker_->getReferenceCloud ()), *result_cloud, transformation);
//Draw blue model reference point cloud
{
pcl::visualization::PointCloudColorHandlerCustom<RefPointType> blue_color (result_cloud, 0, 0, 255);
if (!viz.updatePointCloud (result_cloud, blue_color, "resultcloud"))
viz.addPointCloud (result_cloud, blue_color, "resultcloud");
}
}
bool TransitionLimitXYZRPY::check(FootstepState::Ptr from,
FootstepState::Ptr to) const
{
// from * trans = to
const Eigen::Affine3f diff = to->getPose() * from->getPose().inverse();
const Eigen::Vector3f diff_pos(diff.translation());
if (std::abs(diff_pos[0]) < x_max_ &&
std::abs(diff_pos[1]) < y_max_ &&
std::abs(diff_pos[2]) < z_max_) {
float roll, pitch, yaw;
pcl::getEulerAngles(diff, roll, pitch, yaw);
return (std::abs(roll) < roll_max_ &&
std::abs(pitch) < pitch_max_ &&
std::abs(yaw) < yaw_max_);
}
else {
return false;
}
}
geometry_msgs::Pose transformEigenAffine3fToPose(Eigen::Affine3f e) {
Eigen::Vector3f Oe;
Eigen::Matrix3f Re;
geometry_msgs::Pose pose;
Oe = e.translation();
Re = e.linear();
Eigen::Quaternionf q(Re); // convert rotation matrix Re to a quaternion, q
pose.position.x = Oe(0);
pose.position.y = Oe(1);
pose.position.z = Oe(2);
pose.orientation.x = q.x();
pose.orientation.y = q.y();
pose.orientation.z = q.z();
pose.orientation.w = q.w();
return pose;
}
void GLWidget::paintGL()
{
makeCurrent();
glClearColor(0.7, 0.7, 0.7, 1.0);
glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
glEnable(GL_DEPTH_TEST);
glEnable(GL_BLEND);
if(Interface::showBackgroundImage)
{
// renderTexture.imageAlpha = Interface::alphaBackgroundImage;
// renderTexture.renderTexture(*mTextManagerObj.getBaseTexture(), Eigen::Vector2i(this->width(), this->height()));
}
// phong.render(*mTextManagerObj.getMesh(), *currentCamera, light_trackball);
// multi.render(mTextManagerObj, *currentCamera, light_trackball);
multiMask.useWeights = Interface::useWeights;
multiMask.render(mTextManagerObj, *currentCamera, light_trackball);
// depthMap.render(*mTextManagerObj.getMesh(), calibrationCamera, light_trackball);
if(Interface::camera == Interface::CAMERA_TYPES::FREE)
{
// camera.render();
if(Interface::ShowCameras){
for(int i =0 ; i < mTextManagerObj.getNumPhotos(); i++)
{
mTextManagerObj.changePhotoReferenceTo(i);
camRep.resetModelMatrix();
Eigen::Affine3f m = (*(mTextManagerObj.getViewMatrix())).inverse();
m.translation() -= mTextManagerObj.getMesh()->getCentroid();
m.translation() *= mTextManagerObj.getMesh()->getScale();
m.scale (0.08);
camRep.setModelMatrix(m);
camRep.render(*currentCamera, light_trackball);
}
mTextManagerObj.changePhotoReferenceTo(0);
}
}
}
int main(int argc, char** argv) {
ros::init(argc, argv, "object_finder_node"); // name this node
ROS_INFO("instantiating the object finder action server: ");
ObjectFinder object_finder_as; // create an instance of the class "ObjectFinder"
tf::TransformListener tfListener;
ROS_INFO("listening for kinect-to-base transform:");
tf::StampedTransform stf_kinect_wrt_base;
bool tferr = true;
ROS_INFO("waiting for tf between kinect_pc_frame and base_link...");
while (tferr) {
tferr = false;
try {
//The direction of the transform returned will be from the target_frame to the source_frame.
//Which if applied to data, will transform data in the source_frame into the target_frame.
//See tf/CoordinateFrameConventions#Transform_Direction
tfListener.lookupTransform("base_link", "kinect_pc_frame", ros::Time(0), stf_kinect_wrt_base);
} catch (tf::TransformException &exception) {
ROS_WARN("%s; retrying...", exception.what());
tferr = true;
ros::Duration(0.5).sleep(); // sleep for half a second
ros::spinOnce();
}
}
ROS_INFO("kinect to base_link tf is good");
object_finder_as.xformUtils_.printStampedTf(stf_kinect_wrt_base);
tf::Transform tf_kinect_wrt_base = object_finder_as.xformUtils_.get_tf_from_stamped_tf(stf_kinect_wrt_base);
g_affine_kinect_wrt_base = object_finder_as.xformUtils_.transformTFToAffine3f(tf_kinect_wrt_base);
cout << "affine rotation: " << endl;
cout << g_affine_kinect_wrt_base.linear() << endl;
cout << "affine offset: " << g_affine_kinect_wrt_base.translation().transpose() << endl;
ROS_INFO("going into spin");
// from here, all the work is done in the action server, with the interesting stuff done within "executeCB()"
while (ros::ok()) {
ros::spinOnce(); //normally, can simply do: ros::spin();
ros::Duration(0.1).sleep();
}
return 0;
}
void ViewController::launch(FileData* game, Eigen::Vector3f center)
{
if(game->getType() != GAME)
{
LOG(LogError) << "tried to launch something that isn't a game";
return;
}
Eigen::Affine3f origCamera = mCamera;
origCamera.translation() = -mCurrentView->getPosition();
center += mCurrentView->getPosition();
stopAnimation(1); // make sure the fade in isn't still playing
mLockInput = true;
if(Settings::getInstance()->getString("TransitionStyle") == "fade")
{
// fade out, launch game, fade back in
auto fadeFunc = [this](float t) {
//t -= 1;
//mFadeOpacity = lerp<float>(0.0f, 1.0f, t*t*t + 1);
mFadeOpacity = lerp<float>(0.0f, 1.0f, t);
};
setAnimation(new LambdaAnimation(fadeFunc, 800), 0, [this, game, fadeFunc]
{
game->getSystem()->launchGame(mWindow, game);
mLockInput = false;
setAnimation(new LambdaAnimation(fadeFunc, 800), 0, nullptr, true);
this->onFileChanged(game, FILE_METADATA_CHANGED);
});
}else{
// move camera to zoom in on center + fade out, launch game, come back in
setAnimation(new LaunchAnimation(mCamera, mFadeOpacity, center, 1500), 0, [this, origCamera, center, game]
{
game->getSystem()->launchGame(mWindow, game);
mCamera = origCamera;
mLockInput = false;
setAnimation(new LaunchAnimation(mCamera, mFadeOpacity, center, 600), 0, nullptr, true);
this->onFileChanged(game, FILE_METADATA_CHANGED);
});
}
}
gsl_vector* VelStereoMatcher::stereoToVec(const StereoProperties stereo)
{
Eigen::Affine3f tform = stereo.getLeftCamera().tform;
Eigen::Matrix4f intrinsics = stereo.getLeftCamera().intrinsics.matrix();
Eigen::Vector3f tran;
tran = tform.translation();
float x = tran[0];
float y = tran[1];
float z = tran[2];
Eigen::Matrix3f mat = tform.rotation();
Eigen::AngleAxisf axis;
axis = mat;
float ax = axis.axis()[0] * axis.angle();
float ay = axis.axis()[1] * axis.angle();
float az = axis.axis()[2] * axis.angle();
float fx = intrinsics(0,0);
float fy = intrinsics(1,1);
float cx = intrinsics(0,2);
float cy = intrinsics(1,2);
float baseline = stereo.baseline;
gsl_vector* vec = gsl_vector_alloc(11);
gsl_vector_set(vec, 0, x);
gsl_vector_set(vec, 1, y);
gsl_vector_set(vec, 2, z);
gsl_vector_set(vec, 3, ax);
gsl_vector_set(vec, 4, ay);
gsl_vector_set(vec, 5, az);
gsl_vector_set(vec, 6, fx);
gsl_vector_set(vec, 7, fy);
gsl_vector_set(vec, 8, cx);
gsl_vector_set(vec, 9, cy);
gsl_vector_set(vec, 10, baseline);
return vec;
}
void SLTrackerDialog::showPoseEstimate(const Eigen::Affine3f & T){
if(ui->poseTab->isVisible()){
ui->poseWidget->showPoseEstimate(T);
} else if(ui->traceTab->isVisible()){
Eigen::Vector3f t(T.translation());
Eigen::Quaternionf q(T.rotation());
ui->translationTraceWidget->addMeasurement("tx", t(0));
ui->translationTraceWidget->addMeasurement("ty", t(1));
ui->translationTraceWidget->addMeasurement("tz", t(2));
ui->translationTraceWidget->draw();
ui->rotationTraceWidget->addMeasurement("qw", q.w());
ui->rotationTraceWidget->addMeasurement("qx", q.x());
ui->rotationTraceWidget->addMeasurement("qy", q.y());
ui->rotationTraceWidget->addMeasurement("qz", q.z());
ui->rotationTraceWidget->draw();
}
}
void
drawResult (pcl::visualization::PCLVisualizer& viz)
{
ParticleXYZRPY result = tracker_->getResult ();
Eigen::Affine3f transformation = tracker_->toEigenMatrix (result);
// move a little bit for better visualization
transformation.translation () += Eigen::Vector3f (0.0f, 0.0f, -0.005f);
RefCloudPtr result_cloud (new RefCloud ());
if (!visualize_non_downsample_)
pcl::transformPointCloud<RefPointType> (*(tracker_->getReferenceCloud ()), *result_cloud, transformation);
else
pcl::transformPointCloud<RefPointType> (*reference_, *result_cloud, transformation);
{
pcl::visualization::PointCloudColorHandlerCustom<RefPointType> red_color (result_cloud, 0, 0, 255);
if (!viz.updatePointCloud (result_cloud, red_color, "resultcloud"))
viz.addPointCloud (result_cloud, red_color, "resultcloud");
}
}
void CGLUtil::getRTFromWorld2CamCV(Eigen::Matrix3f* pRw_, Eigen::Vector3f* pTw_) {
//only the matrix in the top of the modelview matrix stack works
Eigen::Affine3f M;
glGetFloatv(GL_MODELVIEW_MATRIX,M.matrix().data());
Eigen::Matrix3f S;
*pTw_ = M.translation();
*pRw_ = M.linear();
//M.computeRotationScaling(pRw_,&S);
//*pTw_ = (*pTw_)/S(0,0);
//negate row no. 1 and 2, to switch from GL to CV convention
for (int r = 1; r < 3; r++){
for (int c = 0; c < 3; c++){
(*pRw_)(r,c) = -(*pRw_)(r,c);
}
(*pTw_)(r) = -(*pTw_)(r);
}
//PRINT(S);
//PRINT(*pRw_);
//PRINT(*pTw_);
return;
}
void affine3fToTransRotVec(const Eigen::Affine3f &aff,
cv::Mat &tvec, cv::Mat &rvec)
{
/* decompose matrix */
Eigen::Vector3f pos = aff.translation();
Eigen::AngleAxisf rot(aff.rotation());
Eigen::Vector3f rotVec = rot.axis();
float angle = rot.angle();
rotVec *= angle;
/* copy translation vector */
tvec = cv::Mat::zeros(3, 1, CV_32F);
tvec.at<float>(0,0) = pos[0];
tvec.at<float>(1,0) = pos[1];
tvec.at<float>(2,0) = pos[2];
/* copy axis angle rotation */
rvec = cv::Mat::zeros(3, 1, CV_32F);
rvec.at<float>(0,0) = rotVec[0];
rvec.at<float>(1,0) = rotVec[1];
rvec.at<float>(2,0) = rotVec[2];
}
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");
}
Eigen::Affine3f createTransMatrix( float tx, float ty, float tz ) {
Eigen::Affine3f m = Eigen::Affine3f::Identity();
m.translation() = Eigen::Vector3f( tx, ty, tz );
return m;
}
void SystemView::render(const Eigen::Affine3f& parentTrans)
{
if(size() == 0)
return;
Eigen::Affine3f trans = getTransform() * parentTrans;
// draw the list elements (titles, backgrounds, logos)
const float logoSizeX = logoSize().x() + LOGO_PADDING;
int logoCount = (int)(mSize.x() / logoSizeX) + 2; // how many logos we need to draw
int center = (int)(mCamOffset);
if(mEntries.size() == 1)
logoCount = 1;
// draw background extras
Eigen::Affine3f extrasTrans = trans;
int extrasCenter = (int)mExtrasCamOffset;
for(int i = extrasCenter - 1; i < extrasCenter + 2; i++)
{
int index = i;
while(index < 0)
index += mEntries.size();
while(index >= (int)mEntries.size())
index -= mEntries.size();
extrasTrans.translation() = trans.translation() + Eigen::Vector3f((i - mExtrasCamOffset) * mSize.x(), 0, 0);
Eigen::Vector2i clipRect = Eigen::Vector2i((int)((i - mExtrasCamOffset) * mSize.x()), 0);
Renderer::pushClipRect(clipRect, mSize.cast<int>());
mEntries.at(index).data.backgroundExtras->render(extrasTrans);
Renderer::popClipRect();
}
// fade extras if necessary
if(mExtrasFadeOpacity)
{
Renderer::setMatrix(trans);
Renderer::drawRect(0.0f, 0.0f, mSize.x(), mSize.y(), 0x00000000 | (unsigned char)(mExtrasFadeOpacity * 255));
}
// draw logos
float xOff = (mSize.x() - logoSize().x())/2 - (mCamOffset * logoSizeX);
float yOff = (mSize.y() - logoSize().y())/2;
// background behind the logos
Renderer::setMatrix(trans);
Renderer::drawRect(0.f, (mSize.y() - BAND_HEIGHT) / 2, mSize.x(), BAND_HEIGHT, 0xFFFFFFD8);
Eigen::Affine3f logoTrans = trans;
for(int i = center - logoCount/2; i < center + logoCount/2 + 1; i++)
{
int index = i;
while(index < 0)
index += mEntries.size();
while(index >= (int)mEntries.size())
index -= mEntries.size();
logoTrans.translation() = trans.translation() + Eigen::Vector3f(i * logoSizeX + xOff, yOff, 0);
if(index == mCursor) //scale our selection up
{
// selected
const std::shared_ptr<GuiComponent>& comp = mEntries.at(index).data.logoSelected;
comp->setOpacity(0xFF);
comp->render(logoTrans);
}else{
// not selected
const std::shared_ptr<GuiComponent>& comp = mEntries.at(index).data.logo;
comp->setOpacity(0x80);
comp->render(logoTrans);
}
}
Renderer::setMatrix(trans);
Renderer::drawRect(mSystemInfo.getPosition().x(), mSystemInfo.getPosition().y() - 1, mSize.x(), mSystemInfo.getSize().y(), 0xDDDDDD00 | (unsigned char)(mSystemInfo.getOpacity() / 255.f * 0xD8));
mSystemInfo.render(trans);
}
// visualizzazione doppia
void VisualizationUtils::vis_doppia( string name1, string name2) {
std::stringstream firstCloud;
firstCloud<<"./../registrazione/cloud"<<name1<<".ply";
std::stringstream secondCloud;
secondCloud<<"./../registrazione/cloud"<<name2<<".ply";
cout<<firstCloud.str()<<endl;
cout<<secondCloud.str()<<endl;
pcl::PointCloud<POINT_TYPE>::Ptr cloud1 (new pcl::PointCloud<POINT_TYPE>);
pcl::PointCloud<POINT_TYPE>::Ptr cloud2 (new pcl::PointCloud<POINT_TYPE>);
pcl::io::loadPLYFile(firstCloud.str().c_str(),*cloud1);
pcl::io::loadPLYFile(secondCloud.str().c_str(),*cloud2);
pcl::PointCloud<POINT_TYPE>::Ptr clicked_points_app(new pcl::PointCloud<POINT_TYPE>);
pcl::PointCloud<POINT_TYPE>::Ptr clicked_points2_app(new pcl::PointCloud<POINT_TYPE>);
// resetto le variabili globali
clicked_points_app->clear();
clicked_points2_app->clear();
points_left.clear_stack();
points_right.clear_stack();
color_left.restart();
color_right.restart();
clicked_points = clicked_points2_app;
clicked_points2 = clicked_points2_app;
// creo la finestra
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
global_viewer = viewer;
// viewer->initCameraParameters();
// viewer->setSize(1200, 650);
// assegno la prima cloud
viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1);
viewer->setCameraPosition(0.0, 0.0, 0.0, 0.0, 0.0, 0.15, 0.0, 1.0, 0.0, v1);
viewer->setBackgroundColor(0.2, 0.2, 0.2, v1); // background grigio
viewer->addText("Prossimo colore: " + color_left.getColorName(), 10, 10, "v1_text", v1);
pcl::visualization::PointCloudColorHandlerRGBField<POINT_TYPE> rgb(cloud1);
viewer->addPointCloud<POINT_TYPE>(cloud1, rgb, name1, v1);
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, name1);
// assegno la seconda cloud
viewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);
viewer->setBackgroundColor(0.3, 0.3, 0.3, v2);
viewer->addText("Prossimo colore: " + color_right.getColorName(), 10, 10, "v2_text", v2);
pcl::visualization::PointCloudColorHandlerRGBField<POINT_TYPE> rgb2(cloud2);
// la traslo per poter identificare i suoi punti
Eigen::Affine3f transform = Eigen::Affine3f::Identity();
transform.translation() << 0.0, 0.0, TRANLSATION_Z_SECOND_CLOUD;
//cout << "matrice applicata alla seconda cloud" << endl << transform.matrix() << endl;
pcl::PointCloud<POINT_TYPE>::Ptr transformed_cloud2(new pcl::PointCloud<POINT_TYPE>);
pcl::transformPointCloud(*cloud2, *transformed_cloud2, transform);
viewer->addPointCloud<POINT_TYPE>(transformed_cloud2, rgb2, name2, v2);
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, name2);
// gestione separata della telecamera
viewer->createViewPortCamera(v2);
viewer->setCameraPosition(0.0, 0.0, TRANLSATION_Z_SECOND_CLOUD, 0.0, 0.0, TRANLSATION_Z_SECOND_CLOUD + 0.15, 0.0, 1.0, 0.0, v2);
// viewer->registerKeyboardCallback(keyboardEventOccurred, (void*)&viewer);
// viewer->registerPointPickingCallback(pointPickDoubleViewEvent, (void*)&viewer);
loop_view(viewer);
// chiusa la view, stampo i punti catturati
cout << "Punti selezionati:" << endl << " -> cloud sinistra:" << endl;
//cout << endl << points_left.makeMatrix() << endl;
points_left.print_all();
cout << " -> cloud destra:" << endl;
//cout << endl << points_right.makeMatrix() << endl;
points_right.print_all();
Eigen::MatrixXf Mx = points_left.makeMatrix();
Eigen::MatrixXf My = points_right.makeMatrix();
if (Mx.cols() != My.cols()) {
cout << "Errore... Hai preso un numero diverso di punti tra destra e sinistra?" << endl;
return ;
}
/*if (Mx.cols() < 3) {
cout << "Dai, sforzati di prendere almeno 3 punti..." << endl;
return;
}*/
Eigen::Matrix4f T = TransformationUtils::trovaT(Mx, My);
ofstream outFile("/home/miky/Scrivania/trasformazione.txt");
outFile << T;
cout << " -> matrice di trasformazione salvata nel file trasformazione.txt" << endl;
pcl::PointCloud<POINT_TYPE>::Ptr result(new pcl::PointCloud<POINT_TYPE>);
pcl::transformPointCloud(*cloud1, *result, T);
pcl::io::savePLYFileBinary("/home/miky/Scrivania/Cloud12.ply", *result);
cout << " -> salvata la cloud traslata con nome Cloud12.ply" << endl;
return ;
}
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_++;
}
/**
* @brief service offering table object candidates
*
* @param req request for objects of a class (table objects in this case)
* @param res response of the service which is possible table object candidates
*
* @return true if service successful
*/
bool
getTablesService2 (cob_3d_mapping_msgs::GetTablesRequest &req,
cob_3d_mapping_msgs::GetTablesResponse &res)
{
ROS_INFO("table detection started....");
cob_3d_mapping_msgs::ShapeArray sa, tables;
if (getMapService (sa))
{
tables.header = sa.header;
//test
tables.header.frame_id = "/map" ;
//end of test
processMap(sa, tables);
for (unsigned int i = 0; i < tables.shapes.size (); i++)
{
//Polygon poly;
Polygon::Ptr poly_ptr(new Polygon());
fromROSMsg(tables.shapes[i], *poly_ptr);
cob_3d_mapping_msgs::Table table;
Eigen::Affine3f pose;
Eigen::Vector4f min_pt;
Eigen::Vector4f max_pt;
poly_ptr->computePoseAndBoundingBox(pose,min_pt, max_pt);
table.pose.pose.position.x = pose.translation()(0); //poly_ptr->centroid[0];
table.pose.pose.position.y = pose.translation()(1) ;//poly_ptr->centroid[1];
table.pose.pose.position.z = pose.translation()(2) ;//poly_ptr->centroid[2];
Eigen::Quaternionf quat(pose.rotation());
// ROS_WARN("poly_ptr->centroid[0]");
// std::cout << poly_ptr->centroid[0]<< "\n";
// ROS_WARN("pose.translation()");
// std::cout << pose.translation() << "\n" ;
table.pose.pose.orientation.x = quat.x();
table.pose.pose.orientation.y = quat.y();
table.pose.pose.orientation.z = quat.z();
table.pose.pose.orientation.w = quat.w();
table.x_min = min_pt(0);
table.x_max = max_pt(0);
table.y_min = min_pt(1);
table.y_max = max_pt(1);
table.convex_hull.type = arm_navigation_msgs::Shape::MESH;
for( unsigned int j=0; j<poly_ptr->contours[0].size(); j++)
{
geometry_msgs::Point p;
p.x = poly_ptr->contours[0][j](0);
p.y = poly_ptr->contours[0][j](1);
p.z = poly_ptr->contours[0][j](2);
table.convex_hull.vertices.push_back(p);
}
cob_3d_mapping_msgs::TabletopDetectionResult det;
det.table = table;
res.tables.push_back(det);
}
// sa_pub_.publish (tables);
publishShapeMarker (tables);
table_ctr_ = tables.shapes.size();
ROS_INFO("Found %d tables", table_ctr_);
return true;
}
else
return false;
}
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 ());
}
}
TEST (PCL, Matrix4Affine3Transform)
{
float rot_x = 2.8827f;
float rot_y = -0.31190f;
float rot_z = -0.93058f;
Eigen::Affine3f affine;
pcl::getTransformation (0, 0, 0, rot_x, rot_y, rot_z, affine);
EXPECT_NEAR (affine (0, 0), 0.56854731f, 1e-4); EXPECT_NEAR (affine (0, 1), -0.82217032f, 1e-4); EXPECT_NEAR (affine (0, 2), -0.028107658f, 1e-4);
EXPECT_NEAR (affine (1, 0), -0.76327348f, 1e-4); EXPECT_NEAR (affine (1, 1), -0.51445758f, 1e-4); EXPECT_NEAR (affine (1, 2), -0.39082864f, 1e-4);
EXPECT_NEAR (affine (2, 0), 0.30686751f, 1e-4); EXPECT_NEAR (affine (2, 1), 0.24365838f, 1e-4); EXPECT_NEAR (affine (2, 2), -0.920034f, 1e-4);
// Approximative!!! Uses SVD internally! See http://eigen.tuxfamily.org/dox/Transform_8h_source.html
Eigen::Matrix3f rotation = affine.rotation ();
EXPECT_NEAR (rotation (0, 0), 0.56854731f, 1e-4); EXPECT_NEAR (rotation (0, 1), -0.82217032f, 1e-4); EXPECT_NEAR (rotation (0, 2), -0.028107658f, 1e-4);
EXPECT_NEAR (rotation (1, 0), -0.76327348f, 1e-4); EXPECT_NEAR (rotation (1, 1), -0.51445758f, 1e-4); EXPECT_NEAR (rotation (1, 2), -0.39082864f, 1e-4);
EXPECT_NEAR (rotation (2, 0), 0.30686751f, 1e-4); EXPECT_NEAR (rotation (2, 1), 0.24365838f, 1e-4); EXPECT_NEAR (rotation (2, 2), -0.920034f, 1e-4);
float trans_x, trans_y, trans_z;
pcl::getTransformation (0.1f, 0.2f, 0.3f, rot_x, rot_y, rot_z, affine);
pcl::getTranslationAndEulerAngles (affine, trans_x, trans_y, trans_z, rot_x, rot_y, rot_z);
EXPECT_FLOAT_EQ (trans_x, 0.1f);
EXPECT_FLOAT_EQ (trans_y, 0.2f);
EXPECT_FLOAT_EQ (trans_z, 0.3f);
EXPECT_FLOAT_EQ (rot_x, 2.8827f);
EXPECT_FLOAT_EQ (rot_y, -0.31190f);
EXPECT_FLOAT_EQ (rot_z, -0.93058f);
Eigen::Matrix4f transformation (Eigen::Matrix4f::Identity ());
transformation.block<3, 3> (0, 0) = affine.rotation ();
transformation.block<3, 1> (0, 3) = affine.translation ();
PointXYZ p (1.f, 2.f, 3.f);
Eigen::Vector3f v3 = p.getVector3fMap ();
Eigen::Vector4f v4 = p.getVector4fMap ();
Eigen::Vector3f v3t (affine * v3);
Eigen::Vector4f v4t (transformation * v4);
PointXYZ pt = pcl::transformPoint (p, affine);
EXPECT_NEAR (pt.x, v3t.x (), 1e-4); EXPECT_NEAR (pt.x, v4t.x (), 1e-4);
EXPECT_NEAR (pt.y, v3t.y (), 1e-4); EXPECT_NEAR (pt.y, v4t.y (), 1e-4);
EXPECT_NEAR (pt.z, v3t.z (), 1e-4); EXPECT_NEAR (pt.z, v4t.z (), 1e-4);
PointNormal pn;
pn.getVector3fMap () = p.getVector3fMap ();
pn.getNormalVector3fMap () = Eigen::Vector3f (0.60f, 0.48f, 0.64f);
Eigen::Vector3f n3 = pn.getNormalVector3fMap ();
Eigen::Vector4f n4 = pn.getNormalVector4fMap ();
Eigen::Vector3f n3t (affine.rotation() * n3);
Eigen::Vector4f n4t (transformation * n4);
PointNormal pnt = pcl::transformPointWithNormal (pn, affine);
EXPECT_NEAR (pnt.x, v3t.x (), 1e-4); EXPECT_NEAR (pnt.x, v4t.x (), 1e-4);
EXPECT_NEAR (pnt.y, v3t.y (), 1e-4); EXPECT_NEAR (pnt.y, v4t.y (), 1e-4);
EXPECT_NEAR (pnt.z, v3t.z (), 1e-4); EXPECT_NEAR (pnt.z, v4t.z (), 1e-4);
EXPECT_NEAR (pnt.normal_x, n3t.x (), 1e-4); EXPECT_NEAR (pnt.normal_x, n4t.x (), 1e-4);
EXPECT_NEAR (pnt.normal_y, n3t.y (), 1e-4); EXPECT_NEAR (pnt.normal_y, n4t.y (), 1e-4);
EXPECT_NEAR (pnt.normal_z, n3t.z (), 1e-4); EXPECT_NEAR (pnt.normal_z, n4t.z (), 1e-4);
PointCloud<PointXYZ> c, ct;
c.push_back (p);
pcl::transformPointCloud (c, ct, affine);
EXPECT_FLOAT_EQ (pt.x, ct[0].x);
EXPECT_FLOAT_EQ (pt.y, ct[0].y);
EXPECT_FLOAT_EQ (pt.z, ct[0].z);
pcl::transformPointCloud (c, ct, transformation);
EXPECT_FLOAT_EQ (pt.x, ct[0].x);
EXPECT_FLOAT_EQ (pt.y, ct[0].y);
EXPECT_FLOAT_EQ (pt.z, ct[0].z);
affine = transformation;
std::vector<int> indices (1); indices[0] = 0;
pcl::transformPointCloud (c, indices, ct, affine);
EXPECT_FLOAT_EQ (pt.x, ct[0].x);
EXPECT_FLOAT_EQ (pt.y, ct[0].y);
EXPECT_FLOAT_EQ (pt.z, ct[0].z);
pcl::transformPointCloud (c, indices, ct, transformation);
EXPECT_FLOAT_EQ (pt.x, ct[0].x);
EXPECT_FLOAT_EQ (pt.y, ct[0].y);
EXPECT_FLOAT_EQ (pt.z, ct[0].z);
}
void cloud_cb (const sensor_msgs::PointCloud2Ptr& input)
{
int is_save = false;
int is_predict = true;
// Create a container for the data.
pcl::PointCloud<pcl::PointXYZI>::Ptr conv_input(new pcl::PointCloud<pcl::PointXYZI>());
input->fields[3].name = "intensity";
pcl::fromROSMsg(*input, *conv_input);
// Size of humansize cloud
int cloud_size = conv_input->points.size();
std::cout << "cloud_size: " << cloud_size << std::endl;
if( cloud_size <= 1 ){
return;
}
pcl::PCA<pcl::PointXYZI> pca;
pcl::PointCloud<pcl::PointXYZI>::Ptr transform_cloud_translate(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr transform_cloud_rotate(new pcl::PointCloud<pcl::PointXYZI>());
sensor_msgs::PointCloud2 output;
geometry_msgs::PointStamped::Ptr output_point(new geometry_msgs::PointStamped());
geometry_msgs::PointStamped output_point_;
Eigen::Vector3f eigen_values;
Eigen::Vector4f centroid;
Eigen::Matrix3f eigen_vectors;
Eigen::Affine3f translate = Eigen::Affine3f::Identity();
Eigen::Affine3f rotate = Eigen::Affine3f::Identity();
double theta;
std::vector<double> description;
description.push_back(cloud_size);
pcl::PointCloud<pcl::PointNormal>::Ptr normals(new pcl::PointCloud<pcl::PointNormal>());
pcl::NormalEstimation<pcl::PointXYZI, pcl::PointNormal> ne;
ne.setInputCloud(conv_input);
ne.setKSearch(24);
ne.compute(*normals);
pcl::FPFHEstimation<pcl::PointXYZI, pcl::PointNormal, pcl::FPFHSignature33> fpfh;
fpfh.setInputCloud(conv_input);
fpfh.setInputNormals(normals);
pcl::search::KdTree<pcl::PointXYZI>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZI>());
fpfh.setSearchMethod(tree);
pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs(new pcl::PointCloud<pcl::FPFHSignature33>());
fpfh.setRadiusSearch(0.05);
fpfh.compute(*fpfhs);
// for(int i=0;i<fpfhs.histogram.size();i++){
std::cout << fpfhs->points.size();
// }
std::cout << std::endl;
//PCA
pca.setInputCloud(conv_input);
centroid = pca.getMean();
std::cout << "Centroid of pointcloud: " << centroid[0] << ", " << centroid[1] << ", " << centroid[2] << std::endl;
eigen_values = pca.getEigenValues();
std::cout << "Eigen values of PCAed pointcloud: " << eigen_values[0] << ", " << eigen_values[1] << ", " << eigen_values[2] << std::endl;
eigen_vectors << pca.getEigenVectors();
std::cout << "Eigen Vectors of PCAed pointcloud:" << std::endl;
for(int i=0;i<eigen_vectors.rows()*eigen_vectors.cols();i++){
std::cout << eigen_vectors(i) << " ";
if(!((i+1)%3)){
std::cout << std::endl;
}
}
std::cout << std::endl;
//Rotation cloud from PCA data
theta = atan2(eigen_vectors(1,0), eigen_vectors(0,0));
std::cout << "Theta: " << theta << std::endl;
translate.translation() << -centroid[0], -centroid[1], -centroid[2];
pcl::transformPointCloud(*conv_input, *transform_cloud_translate, translate);
rotate.rotate(Eigen::AngleAxisf(-theta, Eigen::Vector3f::UnitZ()));
pcl::transformPointCloud(*transform_cloud_translate, *transform_cloud_rotate, rotate);
pcl::toROSMsg(*transform_cloud_rotate, output);
output.header = input -> header;
output.header.frame_id = "map";
pub.publish(output);
pcl::PointXYZI searchPoint;
searchPoint.x = 0.0;
searchPoint.y = 0.0;
searchPoint.z = 0.0;
float min_distance,min_distance_tmp;
min_distance = sqrt(
powf( conv_input->points[0].x - searchPoint.x, 2.0f ) +
powf( conv_input->points[0].y - searchPoint.y, 2.0f ) +
powf( conv_input->points[0].z - searchPoint.z, 2.0f )
);
//brute force nearest neighbor search
for(int i=1; i<conv_input->size(); i++){
min_distance_tmp = sqrt(
powf( conv_input->points[i].x - searchPoint.x, 2.0f ) +
powf( conv_input->points[i].y - searchPoint.y, 2.0f ) +
powf( conv_input->points[i].z - searchPoint.z, 2.0f )
);
if( min_distance < min_distance_tmp){
min_distance = min_distance_tmp;
}
}
std::cout << "min_distance: " << min_distance << std::endl;
std::cout << std::endl;
description.push_back(min_distance);
// Calculate center of mass of humansize cloud
Vector4f center_of_mass;
pcl::compute3DCentroid(*transform_cloud_rotate, center_of_mass);
MatrixXf convariance_matrix = MatrixXf::Zero(3,3);
MatrixXf convariance_matrix_tmp = MatrixXf::Zero(3,3);
MatrixXf moment_of_inertia_matrix_tmp = MatrixXf::Zero(3,3);
MatrixXf moment_of_inertia_matrix = MatrixXf::Zero(3,3);
Vector3f point_tmp;
Vector3f point_from_center_of_mass;
// intensity buffer
double intensity_sum=0, intensity_ave=0, intensity_pow_sum=0, intensity_std_dev=0, max_intensity = 5000;
std::vector<double> intensity_histgram(25);
for(int i=0; i<cloud_size; i++){
point_tmp << transform_cloud_rotate->points[i].x, transform_cloud_rotate->points[i].y, transform_cloud_rotate->points[i].z;
//Calculate three dimentional convariance matrix
point_from_center_of_mass <<
center_of_mass[1] - point_tmp[1],
center_of_mass[0] - point_tmp[0],
center_of_mass[2] - point_tmp[2];
convariance_matrix_tmp += point_tmp * point_tmp.transpose();
//Calculate three dimentional moment of inertia matrix
moment_of_inertia_matrix_tmp <<
powf(point_tmp[1],2.0f)+powf(point_tmp[2],2.0f), -point_tmp[0]*point_tmp[1], -point_tmp[0]*point_tmp[2],
-point_tmp[0]*point_tmp[1], powf(point_tmp[0],2.0f)+powf(point_tmp[2],2.0f), -point_tmp[1]*point_tmp[2],
-point_tmp[0]*point_tmp[2], -point_tmp[1]*point_tmp[2], powf(point_tmp[0],2.0f)+powf(point_tmp[1],2.0f);
moment_of_inertia_matrix = moment_of_inertia_matrix + moment_of_inertia_matrix_tmp;
// Calculate intensity distribution
intensity_sum += transform_cloud_rotate->points[i].intensity;
intensity_pow_sum += powf(transform_cloud_rotate->points[i].intensity,2);
intensity_histgram[transform_cloud_rotate->points[i].intensity / (max_intensity / intensity_histgram.size())] += 1;
if( g_max_inten < transform_cloud_rotate->points[i].intensity ){
g_max_inten = transform_cloud_rotate->points[i].intensity;
}
}
//Calculate Convariance matrix
convariance_matrix = (1.0f/cloud_size) * convariance_matrix_tmp.array();
// Calculate intensity distribution
intensity_ave = intensity_sum / cloud_size;
std::cout << "max_intensity: " << g_max_inten << std::endl;
std::cout << "intensity_ave: " << intensity_ave << std::endl;
intensity_std_dev = sqrt(fabs(intensity_pow_sum / cloud_size - powf(intensity_ave,2)));
std::cout << "intensity_std_dev: " << intensity_std_dev <<std::endl;
std::cout << "intensity_histgram: ";
description.push_back(intensity_ave);
description.push_back(intensity_std_dev);
for(int i=0; i<intensity_histgram.size(); i++){
intensity_histgram[i] = intensity_histgram[i] / cloud_size;
std::cout << intensity_histgram[i] << " ";
description.push_back(intensity_histgram[i]);
}
std::cout << std::endl;
std::cout << std::endl;
std::cout << "3D convariance matrix:" << std::endl;
for(int i=0;i<convariance_matrix.rows()*convariance_matrix.cols();i++){
if(i<5 || i==6){
description.push_back(convariance_matrix(i));
}
std::cout << convariance_matrix(i) << " ";
if(!((i+1)%3)){
std::cout << std::endl;
}
}
std::cout << std::endl;
std::cout << "moment of inertia:" << std::endl;
for(int i=0;i<moment_of_inertia_matrix.rows()*moment_of_inertia_matrix.cols();i++){
if(i<5 || i==6){
description.push_back(convariance_matrix(i));
}
std::cout << moment_of_inertia_matrix(i) << " ";
if(!((i+1)%3)){
std::cout << std::endl;
}
}
std::cout << std::endl;
//Calculate Slice distribution
pcl::PointXYZI min_pt,max_pt,sliced_min_pt,sliced_max_pt;
pcl::getMinMax3D(*transform_cloud_rotate, min_pt, max_pt);
int sectors = 10;
double sector_height = (max_pt.z - min_pt.z)/sectors;
pcl::PassThrough<pcl::PointXYZI> pass;
std::vector<std::vector<double> > slice_dist(sectors,std::vector<double> (2));
for(double sec_h = min_pt.z,i = 0; sec_h < max_pt.z - sector_height; sec_h += sector_height, i++){
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_sliced(new pcl::PointCloud<pcl::PointXYZI>());
pass.setInputCloud(transform_cloud_rotate);
pass.setFilterFieldName("z");
pass.setFilterLimits(sec_h, sec_h + sector_height);
pass.filter(*cloud_sliced);
pcl::getMinMax3D(*cloud_sliced,sliced_min_pt,sliced_max_pt);
slice_dist[i][0] = sliced_max_pt.x - sliced_min_pt.x;
slice_dist[i][1] = sliced_max_pt.y - sliced_min_pt.y;
}
std::cout << "slice distribution:" << std::endl;
for(int i=0;i<2;i++){
for(int j=0;j<10;j++){
description.push_back(slice_dist[j][i]);
std::cout << slice_dist[j][i] << " ";
}
std::cout << std::endl;
}
std::cout << std::endl;
if(is_save){
std::ofstream ofs;
ofs.open("description.txt", std::ios::ate | std::ios::app);
ofs << "1 ";
for(int i=0;i<description.size();i++){
ofs << i+1 << ":" << description[i] << " ";
}
ofs << std::endl;
ofs.close();
}
if(is_predict){
FILE *fp_scale, *fp_model;
int idx,max_index;
std::vector<double> scale_min;
std::vector<double> scale_max;
double scale_min_tmp;
double scale_max_tmp;
double lower,upper;
double predict;
struct svm_model* model = svm_load_model(
model_filename.c_str());
struct svm_node *nodes;
nodes = (struct svm_node *)malloc((description.size()+1)*sizeof(struct svm_node));
fp_scale = fopen(scale_filename.c_str(),"r");
if(fp_scale == NULL || model == NULL){
ROS_ERROR_STREAM("Can't find model or scale data");
return;
}
if(fgetc(fp_scale) != 'x'){
return;
}
fscanf(fp_scale,"%lf %lf",&lower,&upper);
// ROS_INFO_STREAM("lower: " << lower << "upper: "<< upper);
while(fscanf(fp_scale,"%d %lf %lf\n",&idx,&scale_min_tmp,&scale_max_tmp) == 3){
scale_min.push_back(scale_min_tmp);
scale_max.push_back(scale_max_tmp);
}
std::cout << "scaled description :";
int index = 0;
for(int i=0;i<description.size();i++){
if(i<24 || i>28){
if(description[i] == DBL_INF || description[i] == DBL_MINF){
description[i] = 0;
}
if(description[i] < scale_min[index]){
nodes[index].index = i+1;
nodes[index].value = lower;
}
else if(description[i] > scale_max[index]){
nodes[index].index = i+1;
nodes[index].value = upper;
}
else{
nodes[index].index = i+1;
nodes[index].value = lower + (upper-lower) *
(description[i]-scale_min[index])/
(scale_max[index]-scale_min[index]);
}
std::cout << nodes[index].index << ":" << nodes[index].value << ",";
index++;
}
}
nodes[index].index = -1;
std::cout << std::endl;
predict = svm_predict(model,nodes);
ROS_INFO_STREAM("Predict" << predict);
if(predict == 1){
Vector4f center_of_mass_base;
pcl::compute3DCentroid(*conv_input, center_of_mass_base);
output_point->point.x = center_of_mass_base[0];
output_point->point.y = center_of_mass_base[1];
if(std::abs(center_of_mass_base[1]) > 2.0){
if(center_of_mass_base[1] < 0){
output_point->point.y = center_of_mass_base[1] + 1.0;
}
else{
output_point->point.y = center_of_mass_base[1] - 1.0;
}
}
else{
output_point->point.x = center_of_mass_base[0] - 2.0;
}
output_point->point.z = 0.0;
ROS_INFO_STREAM("target_center :" << center_of_mass_base[1] << "," << center_of_mass_base[0] << "," << center_of_mass_base[2]);
output_point->header.frame_id = "base_link";
output_point->header.stamp = input->header.stamp;
try{
listener->transformPoint("/map", *output_point, output_point_);
}
catch(tf::TransformException &e){
ROS_WARN_STREAM("tf::TransformException: " << e.what());
}
pub_point.publish(output_point_);
}
}
}
/**
* @brief Returns the center of the camera in world space.
* @return Camera center
*/
Eigen::Vector3f getCenter (void) const
{
return view_matrix.linear().inverse() * (-view_matrix.translation());
}
void rawCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloud)
{
//double timeScanCur = laserCloud->header.stamp.toSec();
ros::Time timestamp = laserCloud->header.stamp;
//bool returnBool;
pcl::fromROSMsg(*laserCloud, *laserCloudIn);
if (visualizationFlag)
{
viewer->removeAllPointClouds(0);
viewer->removeAllShapes(0);
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGB(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudLabeled(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZI>::Ptr cloudRaw(new pcl::PointCloud<pcl::PointXYZI>);
// std::vector<std::string> labelsC1;
// std::vector<std::string> labelsC2;
// pcl::PointCloud<SAFEFeatures>::Ptr featureCloudC1Acc(new pcl::PointCloud<SAFEFeatures>);
// pcl::PointCloud<SAFEFeatures>::Ptr featureCloudC2Acc(new pcl::PointCloud<SAFEFeatures>);
std::vector<std::string> labelsC1;
std::vector<std::string> labelsC2;
std::vector<std::string> labelsC3;
std::vector<std::string> labels;
pcl::PointCloud<AllFeatures>::Ptr featureCloudAcc(new pcl::PointCloud<AllFeatures>);
pcl::PointCloud<AllFeatures>::Ptr featureCloudC1Acc(new pcl::PointCloud<AllFeatures>);
pcl::PointCloud<AllFeatures>::Ptr featureCloudC2Acc(new pcl::PointCloud<AllFeatures>);
pcl::PointCloud<AllFeatures>::Ptr featureCloudC3Acc(new pcl::PointCloud<AllFeatures>);
std::vector<Eigen::Matrix3f> confMats;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr classificationCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
std::vector<DatasetContainer> dataset;
std::string folder = "/home/mikkel/catkin_ws/src/trainingSet/";
pcl::copyPointCloud(*laserCloudIn, *cloudRGB);
pcl::copyPointCloud(*laserCloudIn, *cloudRaw);
// Single colored
if (visualizationFlag)
{
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> color1(cloudRGB, 0, 0, 255);
viewer->addPointCloud<pcl::PointXYZRGB>(cloudRGB,color1,"cloudRGB",viewP(RawCloud));
}
//viewer->addText ("RawCloud", 10, 10, "vPP text", viewP(RawCloud));
// Ground modeling
// pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudStat(new pcl::PointCloud<pcl::PointXYZRGB>);
// pcl::PointCloud<pcl::PointXYZ>::Ptr planeDistCloud(new pcl::PointCloud<pcl::PointXYZ>);
// pcl::copyPointCloud(*cloudRGB,*cloudStat);
// pcl::copyPointCloud(*cloudRGB,*planeDistCloud);
// Remove everything outside the two lines
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZI>);
//pcl::copyPointCloud(*cloudRaw, *cloud_filtered);
// Rotation
float thetaZ = theta*PI/180; // The angle of rotation in radians
Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
transform_2.rotate (Eigen::AngleAxisf (thetaZ, Eigen::Vector3f::UnitZ()));
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::transformPointCloud (*cloudRaw, *cloud_filtered, transform_2);
// Passthrough
pcl::PassThrough<pcl::PointXYZI> passX;
passX.setInputCloud (cloud_filtered);
passX.setFilterFieldName ("x");
passX.setFilterLimits (-0.5, 100.0);
//pass.setFilterLimitsNegative (true);
passX.filter (*cloud_filtered);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filteredRGB(new pcl::PointCloud<pcl::PointXYZRGB>);
if (receivedHough)
{
pcl::PassThrough<pcl::PointXYZI> pass;
pass.setInputCloud (cloud_filtered);
pass.setFilterFieldName ("y");
pass.setFilterLimits (r1+WALL_CLEARANCE, r2-WALL_CLEARANCE);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_filtered);
}
pcl::copyPointCloud(*cloud_filtered, *cloud_filteredRGB);
if (visualizationFlag)
{
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> color9(cloudRGB, 0, 0, 255);
viewer->addPointCloud<pcl::PointXYZRGB>(cloud_filteredRGB,color9,"LinesFiltered",viewP(LinesFiltered));
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "LinesFiltered");
viewer->addText ("LinesFiltered", 10, 10, fontsize, 1, 1, 1, "LinesFiltered text", viewP(LinesFiltered));
}
// Grid Minimum
// pcl::PointCloud<pcl::PointXYZI>::Ptr gridCloud(new pcl::PointCloud<pcl::PointXYZI>);
// pcl::GridMinimum<pcl::PointXYZI> gridm(1.0); // Set grid resolution
// gridm.setInputCloud(cloud_filtered);
// gridm.filter(*gridCloud);
//*** Transform point cloud to adjust for a Ground Plane ***//
pcl::ModelCoefficients ground_coefficients;
pcl::PointIndices ground_indices;
pcl::SACSegmentation<pcl::PointXYZI> ground_finder;
ground_finder.setOptimizeCoefficients(true);
ground_finder.setModelType(pcl::SACMODEL_PLANE);
ground_finder.setMethodType(pcl::SAC_RANSAC);
ground_finder.setDistanceThreshold(0.15);
ground_finder.setInputCloud(cloud_filtered);
ground_finder.segment(ground_indices, ground_coefficients);
// Convert plane normal vector from type ModelCoefficients to Vector4f
Eigen::Vector3f np = Eigen::Vector3f(ground_coefficients.values[0],ground_coefficients.values[1],ground_coefficients.values[2]);
// Find rotation vector, u, and angle, theta
Eigen::Vector3f u = np.cross(Eigen::Vector3f(0,0,1));
u.normalize();
float theta = acos(np.dot(Eigen::Vector3f(0,0,1)));
// Construct transformation matrix (rotation matrix from axis and angle)
Eigen::Affine3f tf = Eigen::Affine3f::Identity();
float d = ground_coefficients.values[3];
tf.translation() << d*np[0], d*np[1], d*np[2];
tf.rotate (Eigen::AngleAxisf (theta, u));
// Execute the transformation
pcl::PointCloud<pcl::PointXYZI>::Ptr transformedCloud (new pcl::PointCloud<pcl::PointXYZI> ());
pcl::transformPointCloud(*cloud_filtered, *transformedCloud, tf);
// Compute statistical moments for least significant direction in neighborhood
#if (OLD_METHOD)
pcl::NeighborhoodFeatures<pcl::PointXYZI, AllFeatures> features;
pcl::PointCloud<AllFeatures>::Ptr featureCloud(new pcl::PointCloud<AllFeatures>);
features.setInputCloud(transformedCloud);
pcl::search::KdTree<pcl::PointXYZI>::Ptr search_tree5( new pcl::search::KdTree<pcl::PointXYZI>());
features.setSearchMethod(search_tree5);
//principalComponentsAnalysis.setRadiusSearch(0.6);
//features.setKSearch(30);
features.setRadiusSearch(0.01); // This value does not do anything! Look inside "NeighborgoodFeatures.h" to see adaptive radius calculation.
features.compute(*featureCloud);
// ros::Time tFeatureCalculation2 = ros::Time::now();
// ros::Duration tFeatureCalculation = tFeatureCalculation2-tPreprocessing2;
// if (executionTimes==true)
// ROS_INFO("Feature calculation time = %i",tFeatureCalculation.nsec);
visualizeFeature(*cloudRGB, *featureCloud, 0, viewP(GPDistMean), "GPDistMean");
visualizeFeature(*cloudRGB, *featureCloud, 1, viewP(GPDistMin), "GPDistMin");
visualizeFeature(*cloudRGB, *featureCloud, 2, viewP(GPDistPoint), "GPDistPoint");
visualizeFeature(*cloudRGB, *featureCloud, 3, viewP(GPDistVar), "GPDistVar");
visualizeFeature(*cloudRGB, *featureCloud, 4, viewP(PCA1), "PCA1"); // 4 = PCA1
visualizeFeature(*cloudRGB, *featureCloud, 5, viewP(PCA2), "PCA2"); // 3 = GPVar
visualizeFeature(*cloudRGB, *featureCloud, 6, viewP(PCA3), "PCA3"); // 0 = GPMean
visualizeFeature(*cloudRGB, *featureCloud, 7, viewP(PCANX), "PCANX");
visualizeFeature(*cloudRGB, *featureCloud, 8, viewP(PCANY), "PCANY");
visualizeFeature(*cloudRGB, *featureCloud, 9, viewP(PCANZ), "PCANZ"); // 9 = PCANZ
visualizeFeature(*cloudRGB, *featureCloud, 10, viewP(PointDist), "PointDist");
visualizeFeature(*cloudRGB, *featureCloud, 11, viewP(RSS), "RSS");
visualizeFeature(*cloudRGB, *featureCloud, 12, viewP(Reflectance), "Reflectance");
ROS_INFO("Computed all neighborhood features");
std::vector<float>* centroid = new std::vector<float>();
std::vector<float>* stds = new std::vector<float>();
//*** Testing ***//
if (training == false)
{
Eigen::Matrix3f confMat = Eigen::Matrix3f::Zero();
if (testdata==true)
{
*featureCloud = *featureCloudAcc;
}
pcl::copyPointCloud(*cloudRGB,*classificationCloud);
// Load feature normalization parameters
std::ifstream input_file((folder + "centroid").c_str());
std::istream_iterator<float> start(input_file), end;
std::vector<float> numbers(start, end);
std::copy(numbers.begin(), numbers.end(), std::back_inserter(*centroid));
std::ifstream input_file2((folder + "stds").c_str());
std::istream_iterator<float> start2(input_file2), end2;
std::vector<float> numbers2(start2, end2);
std::copy(numbers2.begin(), numbers2.end(), std::back_inserter(*stds));
// Normalize features
// if (pcl::io::savePCDFile ("testFeaturesNonNormalized.pcd", *featureCloud) != 0)
// return (false);
ros::Time tNormalization1 = ros::Time::now();
normalizeFeatures(*featureCloud,*featureCloud, ¢roid, &stds);
ros::Time tNormalization2 = ros::Time::now();
ros::Duration tNormalization = tNormalization2-tNormalization1;
if (executionTimes==true)
ROS_INFO("Normalization time = %f",(float)(tNormalization.nsec)/1000000);
pcl::PointIndices::Ptr unclassifiedIndices (new pcl::PointIndices ());
// if (pcl::io::savePCDFile ("testFeatures.pcd", *featureCloud) != 0)
// return (false);
CvSVM SVM;
//int dim = sizeof(featureCloud->points[0])/sizeof(float);
int dim = useFeatures.size();//sizeof(useFeatures)/sizeof(int);
SVM.load((folder + "svm").c_str());
//SVM.load((folder + "svm2014-11-06-11-22-59").c_str());
//SVM.load((folder + "svm2014-11-07-14-00-31").c_str());
//SVM.load((folder + "svm2014-11-07-13-16-28").c_str());
ros::Time tClassification1 = ros::Time::now();
//int nMissingLabels = 0;
for (size_t i=0;i<classificationCloud->points.size();i++)
{
float dataSVM[dim];
for (int d=0;d<dim;d++)
{
//dataSVM[d] = featureCloud->points[i].data[d];
dataSVM[d] = featureCloud->points[i].data[useFeatures[d]];
}
Mat labelsMat(1, dim, CV_32FC1, dataSVM);
float response = SVM.predict(labelsMat);
if (response == 1.0)
classificationCloud->points[i].b = 255;
else if (response == 2.0)
classificationCloud->points[i].g = 255;
else if (response == 3.0)
classificationCloud->points[i].r = 255;
else
ROS_INFO("Another class label = %f",response);
if (testdata==true)
{
int groundTruth;
if (labels[i].compare("ground") == 0)
groundTruth = 1;
else if (labels[i].compare("vegetation") == 0)
groundTruth = 2;
else if (labels[i].compare("object") == 0)
groundTruth = 3;
confMat(groundTruth-1,(int)response-1)++;
}
}
ros::Time tClassification2 = ros::Time::now();
ros::Duration tClassification = tClassification2-tClassification1;
if (executionTimes==true)
ROS_INFO("Classification time = %f",(float)(tClassification.nsec)/100000);
//ROS_INFO("Number of missing class labels = %i", nMissingLabels);
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> colorC(classificationCloud);
viewer->addPointCloud<pcl::PointXYZRGB>(classificationCloud,colorC,"Classification",viewP(Classification));
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "Classification");
viewer->addText ("Classification", 10, 10, fontsize, 1, 1, 1, "Classification text", viewP(Classification));
// Test set - calculate performance statistics
if (testdata == true)
{
std::cout << confMat << std::endl;
confMats.push_back(confMat);
if (true)//k==files.size()-1)
{
ofstream myfile;
string resultsFolder = "/home/mikkel/catkin_ws/src/Results/";
myfile.open ((resultsFolder + "run_number_here.txt").c_str());
// Distance threshold
myfile << "DistanceThreshold:" << distThr << ";\n";
// Neighborhood parameters
myfile << "Neighborhood:" << rmin << ";" << rmindist << ";" << rmax << ";" << rmaxdist << ";\n";
// Features
myfile << "Features:";
for (int d=0;d<dim;d++)
myfile << useFeatures[d] << ";";
myfile << "\n";
myfile << "ConfusionMatrix:";
for (size_t i=0;i<confMats.size();i++)
{
for (int r=0;r<confMats[i].rows();r++)
for (int c=0;c<confMats[i].cols();c++)
myfile << confMats[i](r,c) << ";";
myfile << "\n";
}
myfile << "Accuracy:" << confMat.diagonal().sum()/confMat.sum() << ";\n";
myfile.close();
}
}
featureCloudAcc->clear();
labels.clear();
#else
for (size_t i=0;i<transformedCloud->size();i++)
{
pcl::PointXYZI pTmp2 = transformedCloud->points[i];
pcl::PointXYZRGB pTmp;
pTmp.x = pTmp2.x;
pTmp.y = pTmp2.y;
pTmp.z = pTmp2.z;
pTmp.r = 255;
pTmp.g = 255;
if (pTmp.z > 0.1) // Object
{
classificationCloud->push_back(pTmp);
}
}
if (visualizationFlag)
{
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> colorC(classificationCloud);
viewer->addPointCloud<pcl::PointXYZRGB>(classificationCloud,colorC,"Classification",viewP(Classification));
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "Classification");
viewer->addText ("Classification", 10, 10, fontsize, 1, 1, 1, "Classification text", viewP(Classification));
}
#endif
// REGION GROWING
//ROS_INFO("region growing 5");
pcl::PointCloud <pcl::PointXYZRGB>::Ptr objectCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
//pcl::PointCloud <pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud <pcl::PointXYZRGB>);
for (size_t i=0;i<classificationCloud->size();i++)
{
pcl::PointXYZRGB pTmp = classificationCloud->points[i];
if ((pTmp.r == 255) || (pTmp.g == 255)) // Object
{
objectCloud->push_back(pTmp);
}
}
pcl::RegionGrowing<pcl::PointXYZRGB, pcl::Normal> reg;
reg.setInputCloud (objectCloud);
//reg.setIndices (indices);
pcl::search::Search<pcl::PointXYZRGB>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZRGB> > (new pcl::search::KdTree<pcl::PointXYZRGB>);
reg.setSearchMethod (tree);
//reg.setDistanceThreshold (0.0001f);
//reg.setResidualThreshold(0.01f);
//ROS_INFO("res = %f",reg.getSmoothnessThreshold());
//reg.setPointColorThreshold (6);
//reg.setRegionColorThreshold (5);
reg.setMinClusterSize (1);
reg.setResidualThreshold(min_cluster_distance);
reg.setCurvatureTestFlag(false);
// reg.setResidualTestFlag(true);
// reg.setNormalTestFlag(false);
// reg.setSmoothModeFlag(false);
std::vector <pcl::PointIndices> clusters;
reg.extract (clusters);
//ROS_INFO("clusters = %d", clusters.size());
//std::cout << "testefter" << std::endl;
pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = reg.getColoredCloud ();
pcl::PointCloud <pcl::PointXYZRGB>::Ptr clustersFilteredCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
std::vector <pcl::PointIndices> clustersFiltered;
//pcl::PointXYZRGB min_pt, max_pt;
Eigen::Vector4f min_pt, max_pt;
obstacle_detection::boundingbox bbox;
obstacle_detection::boundingboxes bboxes;
bboxes.header.stamp = timestamp;
bboxes.header.frame_id = "/velodyne";
if (visualizationFlag)
viewer->removeAllShapes(viewP(SegmentsFiltered));
for (size_t i=0;i<clusters.size();i++)
{
//ROS_INFO("cluster size = %d",clusters[i].indices.size());
if (clusters[i].indices.size() > 100)
{
clustersFiltered.push_back(clusters[i]);
for (size_t pp=0;pp<clusters[i].indices.size();pp++)
{
clustersFilteredCloud->push_back(colored_cloud->points[clusters[i].indices[pp]]);
}
// Calculate bounding box
pcl::getMinMax3D(*colored_cloud, clusters[i], min_pt, max_pt);
bbox.start.x = min_pt[0];
bbox.start.y = min_pt[1];
bbox.start.z = min_pt[2];
bbox.width.x = max_pt[0]-min_pt[0];
bbox.width.y = max_pt[1]-min_pt[1];
bbox.width.z = max_pt[2]-min_pt[2];
bbox.header.stamp = timestamp;
bbox.header.frame_id = "/velodyne";
bboxes.boundingboxes.push_back(bbox);
if (visualizationFlag)
viewer->addCube (min_pt[0], max_pt[0], min_pt[1], max_pt[1], min_pt[2], max_pt[2], 1.0f, 1.0f, 1.0f, (boost::format("%04d") % i).str().c_str(), viewP(SegmentsFiltered));
}
}
pubBBoxesPointer->publish(bboxes);
//
// //pcl::visualization::CloudViewer viewer ("Cluster viewer");
// //viewer.showCloud (colored_cloud);
//
if (visualizationFlag)
{
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> colorC2(colored_cloud);
viewer->addPointCloud<pcl::PointXYZRGB>(colored_cloud,colorC2,"Segments",viewP(Segments));
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "Segments");
viewer->addText ("Segments", 10, 10, fontsize, 1, 1, 1, "Segments text", viewP(Segments));
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> colorC3(clustersFilteredCloud);
viewer->addPointCloud<pcl::PointXYZRGB>(clustersFilteredCloud,colorC3,"SegmentsFiltered",viewP(SegmentsFiltered));
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "SegmentsFiltered");
viewer->addText ("Segments", 10, 10, fontsize, 1, 1, 1, "SegmentsFiltered text", viewP(SegmentsFiltered));
}
//
//
// // BOUNDING BOXES
// viewer->addCube (min_pt.x, max_pt.x, min_pt.y, max_pt.y, min_pt.z, max_pt.z);
// std_msgs::Float64 testF;
// //testF.data = 10;
// testF.data = 10;
//
// Compute principal directions
// Eigen::Vector4f pcaCentroid;
// pcl::compute3DCentroid(*clustersFilteredCloud, pcaCentroid);
// Eigen::Matrix3f covariance;
// computeCovarianceMatrixNormalized(*clustersFilteredCloud, pcaCentroid, covariance);
// Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);
// Eigen::Matrix3f eigenVectorsPCA = eigen_solver.eigenvectors();
// eigenVectorsPCA.col(2) = eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1));
//
// // Transform the original cloud to the origin where the principal components correspond to the axes.
// Eigen::Matrix4f projectionTransform(Eigen::Matrix4f::Identity());
// projectionTransform.block<3,3>(0,0) = eigenVectorsPCA.transpose();
// projectionTransform.block<3,1>(0,3) = -1.f * (projectionTransform.block<3,3>(0,0) * pcaCentroid.head<3>());
// pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudPointsProjected (new pcl::PointCloud<pcl::PointXYZRGB>);
// pcl::transformPointCloud(*clustersFilteredCloud, *cloudPointsProjected, projectionTransform);
// // Get the minimum and maximum points of the transformed cloud.
// pcl::PointXYZRGB minPoint, maxPoint;
// pcl::getMinMax3D(*cloudPointsProjected, minPoint, maxPoint);
// const Eigen::Vector3f meanDiagonal = 0.5f*(maxPoint.getVector3fMap() + minPoint.getVector3fMap());
//
// // Final transform
// const Eigen::Quaternionf bboxQuaternion(eigenVectorsPCA); //Quaternions are a way to do rotations https://www.youtube.com/watch?v=mHVwd8gYLnI
// const Eigen::Vector3f bboxTransform = eigenVectorsPCA * meanDiagonal + pcaCentroid.head<3>();
// viewer->addCube(bboxTransform, bboxQuaternion, maxPoint.x - minPoint.x, maxPoint.y - minPoint.y, maxPoint.z - minPoint.z, "bbox");
//pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> colorTTF1(featureCloudTest, 0, 255, 0);
// pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> colorTTF1(featureCloudTest);
// PCL_INFO("featureCloudTest size = %i",featureCloudTest->points.size());
// pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> colorTTF2(featureCloudTrain, 255, 0, 0);
// viewer->addPointCloud<pcl::PointXYZRGB>(featureCloudTest,colorTTF1,"TestTrainFeatures1",viewP(TestTrainFeatures));
// viewer->addPointCloud<pcl::PointXYZRGB>(featureCloudTrain,colorTTF2,"TestTrainFeatures2",viewP(TestTrainFeatures));
// viewer->addText ("TestTrainFeatures", 10, 10, "TestTrainFeatures text", viewP(TestTrainFeatures));
// sensor_msgs::PointCloud2 processedCloud;
// pcl::toROSMsg(*classificationCloud, processedCloud);
// processedCloud.header.stamp = ros::Time::now();//processedCloud->header.stamp;
// processedCloud.header.frame_id = "/velodyne";
// pubProcessedPointCloudPointer->publish(processedCloud);
//
// pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud2D(new pcl::PointCloud<pcl::PointXYZRGB>);
// pcl::PointCloud<pcl::PointXYZRGB>::Ptr objectCloud2D(new pcl::PointCloud<pcl::PointXYZRGB>);
//
// for (size_t i=0;i<classificationCloud->size();i++)
// {
// pcl::PointXYZRGB pTmp = classificationCloud->points[i];
// pTmp.z = 0; // 3D -> 2D
// if ((pTmp.r == 255) || (pTmp.g == 255)) // Object
// {
// objectCloud2D->push_back(pTmp);
// }
// else if (pTmp.b == 255) // Object
// {
// groundCloud2D->push_back(pTmp);
// }
// }
//
// //std::vector<int> indexVector;
// unsigned int leafNodeCounter = 0;
// unsigned int voxelDensity = 0;
// unsigned int totalPoints = 0;
// //int occupancyW = OCCUPANCY_WIDTH/OCCUPANCY_GRID_RESOLUTION;
// //int occupancyH = OCCUPANCY_DEPTH/OCCUPANCY_GRID_RESOLUTION;
// std::vector<unsigned int> ocGroundGrid(OCCUPANCY_WIDTH*OCCUPANCY_DEPTH, 0);
// std::vector<unsigned int> ocObjectGrid(OCCUPANCY_WIDTH*OCCUPANCY_DEPTH, 0);
// std::vector<signed char> ocGridFinal(OCCUPANCY_WIDTH*OCCUPANCY_DEPTH,-1);
//
// // Ground grid
// pcl::octree::OctreePointCloudDensity< pcl::PointXYZRGB > octreeGround (OCCUPANCY_GRID_RESOLUTION);
// octreeGround.setInputCloud (groundCloud2D);
// pcl::PointXYZ bot(-OCCUPANCY_DEPTH_M/2,-OCCUPANCY_WIDTH_M/2,-0.5);
// pcl::PointXYZ top(OCCUPANCY_DEPTH_M/2,OCCUPANCY_WIDTH_M/2,0.5);
// octreeGround.defineBoundingBox(bot.x, bot.y, bot.z, top.x, top.y, top.z); // I have already calculated the BBox of my point cloud
// octreeGround.addPointsFromInputCloud ();
// pcl::octree::OctreePointCloudDensity<pcl::PointXYZRGB>::LeafNodeIterator it1;
// pcl::octree::OctreePointCloudDensity<pcl::PointXYZRGB>::LeafNodeIterator it1_end = octreeGround.leaf_end();
// int minx = 1000;
// int miny = 1000;
// int maxx = -1000;
// int maxy = -1000;
// for (it1 = octreeGround.leaf_begin(); it1 != it1_end; ++it1)
// {
// pcl::octree::OctreePointCloudDensityContainer& container = it1.getLeafContainer();
// voxelDensity = container.getPointCounter();
// Eigen::Vector3f min_pt, max_pt;
// octreeGround.getVoxelBounds (it1, min_pt, max_pt);
// //ROS_INFO("x = %f, y = %f, z = %f, x2 = %f, y2 = %f, z2 = %f",min_pt[0],min_pt[1],min_pt[2],max_pt[0],max_pt[1],max_pt[2]);
// totalPoints += voxelDensity;
// if (min_pt[0] < minx)
// minx = min_pt[0];
// if (min_pt[0] > maxx)
// maxx = min_pt[0];
// if (min_pt[1] < miny)
// miny = min_pt[1];
// if (min_pt[1] > maxy)
// maxy = min_pt[1];
//
// int r = OCCUPANCY_DEPTH-(min_pt[0]+OCCUPANCY_DEPTH_M/2)/OCCUPANCY_GRID_RESOLUTION;
// int c = OCCUPANCY_WIDTH-(min_pt[1]+OCCUPANCY_WIDTH_M/2)/OCCUPANCY_GRID_RESOLUTION;
//
// // if ((r == 51) || (c==51))
// // ROS_INFO("r = %i,c = %i",r,c);
//
// //if (((int)min_pt[0] == -1) || ((int)min_pt[1]==-1))
// //ROS_INFO("min_pt[0] = %f, min_pt[1]=%f",min_pt[0],min_pt[1]);
// if (!((r < 0) || (c < 0) || (r > OCCUPANCY_DEPTH-1) || (c > OCCUPANCY_WIDTH-1)))
// ocGroundGrid[r+OCCUPANCY_DEPTH*c] = voxelDensity;
// //ROS_INFO("density = %i, r = %i, c=%i",voxelDensity, r, c);
// //ROS_INFO("x (%f) -> r (%i), y (%f) -> c (%i)",min_pt[0],r,min_pt[1],c);
// leafNodeCounter++;
// }
//
//
// // Object grid
// pcl::octree::OctreePointCloudDensity< pcl::PointXYZRGB > octreeObject (OCCUPANCY_GRID_RESOLUTION);
// octreeObject.setInputCloud (objectCloud2D);
// octreeObject.defineBoundingBox(bot.x, bot.y, bot.z, top.x, top.y, top.z); // I have already calculated the BBox of my point cloud
// octreeObject.addPointsFromInputCloud ();
// pcl::octree::OctreePointCloudDensity<pcl::PointXYZRGB>::LeafNodeIterator it2;
// pcl::octree::OctreePointCloudDensity<pcl::PointXYZRGB>::LeafNodeIterator it2_end = octreeObject.leaf_end();
// minx = 1000;
// miny = 1000;
// maxx = -1000;
// maxy = -1000;
// leafNodeCounter = 0;
// voxelDensity = 0;
// totalPoints = 0;
// int t1 = 0;
// int t2 = 0;
// int t3 = 0;
// for (it2 = octreeObject.leaf_begin(); it2 != it2_end; ++it2)
// {
// pcl::octree::OctreePointCloudDensityContainer& container = it2.getLeafContainer();
// voxelDensity = container.getPointCounter();
// Eigen::Vector3f min_pt, max_pt;
// octreeObject.getVoxelBounds (it2, min_pt, max_pt);
// //ROS_INFO("x = %f, y = %f, z = %f, x2 = %f, y2 = %f, z2 = %f",min_pt[0],min_pt[1],min_pt[2],max_pt[0],max_pt[1],max_pt[2]);
// totalPoints += voxelDensity;
// if (min_pt[0] < minx)
// minx = min_pt[0];
// if (min_pt[0] > maxx)
// maxx = min_pt[0];
// if (min_pt[1] < miny)
// miny = min_pt[1];
// if (min_pt[1] > maxy)
// maxy = min_pt[1];
//
// int r = OCCUPANCY_DEPTH-(min_pt[0]+OCCUPANCY_DEPTH_M/2)/OCCUPANCY_GRID_RESOLUTION;
// int c = OCCUPANCY_WIDTH-(min_pt[1]+OCCUPANCY_WIDTH_M/2)/OCCUPANCY_GRID_RESOLUTION;
// if (!((r < 0) || (c < 0) || (r > OCCUPANCY_DEPTH-1) || (c > OCCUPANCY_WIDTH-1)))
// ocObjectGrid[r+OCCUPANCY_DEPTH*c] = voxelDensity;
// //ROS_INFO("object points = %i -> %i: x (%f) -> r (%i), y (%f) -> c (%i)",voxelDensity,ocGridFinal[r+OCCUPANCY_DEPTH*c],min_pt[0],r,min_pt[1],c);
// //ocGrid[r+OCCUPANCY_DEPTH*c] = voxelDensity;
// //ROS_INFO("density = %i, r = %i, c=%i",voxelDensity, r, c);
//
// leafNodeCounter++;
// }
//
// for (int r=0;r<OCCUPANCY_DEPTH;r++)
// {
// for (int c=0;c<OCCUPANCY_WIDTH;c++)
// {
// if ((ocGroundGrid[r+OCCUPANCY_DEPTH*c] == 0) && (ocObjectGrid[r+OCCUPANCY_DEPTH*c] <= 1))
// {
// ocGridFinal[r+OCCUPANCY_DEPTH*c] = -1.0; // 0.5 default likelihood
// t1++;
// }
// else if ((ocObjectGrid[r+OCCUPANCY_DEPTH*c] == 0))// && (ocGroundGrid[r+OCCUPANCY_DEPTH*c] > 0))
// {
// ocGridFinal[r+OCCUPANCY_DEPTH*c] = OCCUPANCY_MIN_PROB;
// t2++;
// }
// else
// {
// ocGridFinal[r+OCCUPANCY_DEPTH*c] = 50+(OCCUPANCY_MAX_PROB-50)*ocObjectGrid[r+OCCUPANCY_DEPTH*c]/(ocObjectGrid[r+OCCUPANCY_DEPTH*c]+ocGroundGrid[r+OCCUPANCY_DEPTH*c]);
// t3++;
// }
// }
// }
//
// nav_msgs::OccupancyGrid occupancyGrid;
// occupancyGrid.info.resolution = OCCUPANCY_GRID_RESOLUTION;
// occupancyGrid.header.stamp = timestamp;//ros::Time::now();
// occupancyGrid.header.frame_id = "/velodyne";
// occupancyGrid.info.width = OCCUPANCY_WIDTH;
// occupancyGrid.info.height = OCCUPANCY_DEPTH;
// //geometry_msgs::Pose lidarPose;
// geometry_msgs::Point lidarPoint;
// lidarPoint.x = 0;
// lidarPoint.y = 0;
// lidarPoint.z = 0;
// geometry_msgs::Quaternion lidarQuaternion;
// lidarQuaternion.w = 1;
// lidarQuaternion.x = 0;
// lidarQuaternion.y = 0;
// lidarQuaternion.z = 0;
// occupancyGrid.info.origin.position = lidarPoint;
// occupancyGrid.info.origin.orientation = lidarQuaternion;
// occupancyGrid.data = ocGridFinal;
//
// //ROS_INFO("total points = %i, total leaves = %i",totalPoints,leafNodeCounter);
// //ROS_INFO("minx = %i, maxx = %i, miny = %i, maxy = %i",minx,maxx,miny,maxy);
// //ROS_INFO("t1 = %i, t2 = %i, t3 = %i",t1,t2,t3);
//
//
// pubOccupancyGridPointer->publish(occupancyGrid);
// int l = 0;
// while (*++itLeafs)
// {
// //Iteratively explore only the leaf nodes..
// std::vector<PointXYZ> points;
// itLeafs->getData(points);
// l++;
// }
//
// ROS_INFO("l = %i",l);
//pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree (OCCUPANCY_GRID_RESOLUTION);
// sensor_msgs::PointCloud2::Ptr cloud_filtered (new sensor_msgs::PointCloud2 ());
// pcl::VoxelGrid<sensor_msgs::PointCloud2> sor;
// sor.setInputCloud (processedCloud);
// sor.setLeafSize (0.01f, 0.01f, 0.01f);
// sor.filter (*cloud_filtered);
if (n==0)
{
//viewer->setCameraPosition(-20,0,10,1,0,2,1,0,2,v1);
//viewer->setCameraPosition(-20,0,10,1,0,2,1,0,2,v2);
//viewer->loadCameraParameters("pcl_video.cam");
}
#if (OLD_METHOD)
}
#endif
if (visualizationFlag)
viewer->spinOnce();
// Save screenshot
// std::stringstream tmp;
// tmp << n;
// viewer->saveScreenshot("/home/mikkel/images/" + (boost::format("%04d") % n).str() + ".png");
// n++;
}
void PointCloudLocalization::cloudCallback(
const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
{
vital_checker_->poke();
boost::mutex::scoped_lock lock(mutex_);
//JSK_NODELET_INFO("cloudCallback");
latest_cloud_ = cloud_msg;
if (localize_requested_){
JSK_NODELET_INFO("localization is requested");
try {
pcl::PointCloud<pcl::PointNormal>::Ptr
local_cloud (new pcl::PointCloud<pcl::PointNormal>);
pcl::fromROSMsg(*latest_cloud_, *local_cloud);
JSK_NODELET_INFO("waiting for tf transformation from %s tp %s",
latest_cloud_->header.frame_id.c_str(),
global_frame_.c_str());
if (tf_listener_->waitForTransform(
latest_cloud_->header.frame_id,
global_frame_,
latest_cloud_->header.stamp,
ros::Duration(1.0))) {
pcl::PointCloud<pcl::PointNormal>::Ptr
input_cloud (new pcl::PointCloud<pcl::PointNormal>);
if (use_normal_) {
pcl_ros::transformPointCloudWithNormals(global_frame_,
*local_cloud,
*input_cloud,
*tf_listener_);
}
else {
pcl_ros::transformPointCloud(global_frame_,
*local_cloud,
*input_cloud,
*tf_listener_);
}
pcl::PointCloud<pcl::PointNormal>::Ptr
input_downsampled_cloud (new pcl::PointCloud<pcl::PointNormal>);
applyDownsampling(input_cloud, *input_downsampled_cloud);
if (isFirstTime()) {
all_cloud_ = input_downsampled_cloud;
first_time_ = false;
}
else {
// run ICP
ros::ServiceClient client
= pnh_->serviceClient<jsk_pcl_ros::ICPAlign>("icp_align");
jsk_pcl_ros::ICPAlign icp_srv;
if (clip_unseen_pointcloud_) {
// Before running ICP, remove pointcloud where we cannot see
// First, transform reference pointcloud, that is all_cloud_, into
// sensor frame.
// And after that, remove points which are x < 0.
tf::StampedTransform global_sensor_tf_transform
= lookupTransformWithDuration(
tf_listener_,
global_frame_,
sensor_frame_,
cloud_msg->header.stamp,
ros::Duration(1.0));
Eigen::Affine3f global_sensor_transform;
tf::transformTFToEigen(global_sensor_tf_transform,
global_sensor_transform);
pcl::PointCloud<pcl::PointNormal>::Ptr sensor_cloud
(new pcl::PointCloud<pcl::PointNormal>);
pcl::transformPointCloudWithNormals(
*all_cloud_,
*sensor_cloud,
global_sensor_transform.inverse());
// Remove negative-x points
pcl::PassThrough<pcl::PointNormal> pass;
pass.setInputCloud(sensor_cloud);
pass.setFilterFieldName("x");
pass.setFilterLimits(0.0, 100.0);
pcl::PointCloud<pcl::PointNormal>::Ptr filtered_cloud
(new pcl::PointCloud<pcl::PointNormal>);
pass.filter(*filtered_cloud);
JSK_NODELET_INFO("clipping: %lu -> %lu", sensor_cloud->points.size(), filtered_cloud->points.size());
// Convert the pointcloud to global frame again
pcl::PointCloud<pcl::PointNormal>::Ptr global_filtered_cloud
(new pcl::PointCloud<pcl::PointNormal>);
pcl::transformPointCloudWithNormals(
*filtered_cloud,
*global_filtered_cloud,
global_sensor_transform);
pcl::toROSMsg(*global_filtered_cloud,
icp_srv.request.target_cloud);
}
else {
pcl::toROSMsg(*all_cloud_,
icp_srv.request.target_cloud);
}
pcl::toROSMsg(*input_downsampled_cloud,
icp_srv.request.reference_cloud);
if (client.call(icp_srv)) {
Eigen::Affine3f transform;
tf::poseMsgToEigen(icp_srv.response.result.pose, transform);
Eigen::Vector3f transform_pos(transform.translation());
float roll, pitch, yaw;
pcl::getEulerAngles(transform, roll, pitch, yaw);
JSK_NODELET_INFO("aligned parameter --");
JSK_NODELET_INFO(" - pos: [%f, %f, %f]",
transform_pos[0],
transform_pos[1],
transform_pos[2]);
JSK_NODELET_INFO(" - rot: [%f, %f, %f]", roll, pitch, yaw);
pcl::PointCloud<pcl::PointNormal>::Ptr
transformed_input_cloud (new pcl::PointCloud<pcl::PointNormal>);
if (use_normal_) {
pcl::transformPointCloudWithNormals(*input_cloud,
*transformed_input_cloud,
transform);
}
else {
pcl::transformPointCloud(*input_cloud,
*transformed_input_cloud,
transform);
}
pcl::PointCloud<pcl::PointNormal>::Ptr
concatenated_cloud (new pcl::PointCloud<pcl::PointNormal>);
*concatenated_cloud = *all_cloud_ + *transformed_input_cloud;
// update *all_cloud
applyDownsampling(concatenated_cloud, *all_cloud_);
// update localize_transform_
tf::Transform icp_transform;
tf::transformEigenToTF(transform, icp_transform);
{
boost::mutex::scoped_lock tf_lock(tf_mutex_);
localize_transform_ = localize_transform_ * icp_transform;
}
}
else {
JSK_NODELET_ERROR("Failed to call ~icp_align");
return;
}
}
localize_requested_ = false;
}
else {
JSK_NODELET_WARN("No tf transformation is available");
}
}
catch (tf2::ConnectivityException &e)
{
JSK_NODELET_ERROR("[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
return;
}
catch (tf2::InvalidArgumentException &e)
{
JSK_NODELET_ERROR("[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
return;
}
}
}
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_++;
}
/**
* @brief Returns the translation part of the view matrix as a vector.
* @return The translation part of the view matrix.
*/
Eigen::Vector3f getTranslationMatrix (void) const
{
return view_matrix.translation();
}