TEST (PCL, WarpPointRigid6DFloat)
{
WarpPointRigid6D<PointXYZ, PointXYZ, float> warp;
Eigen::Quaternionf q (0.4455f, 0.9217f, 0.3382f, 0.3656f);
q.normalize ();
Eigen::Vector3f t (0.82550f, 0.11697f, 0.44864f);
Eigen::VectorXf p (6);
p[0] = t.x (); p[1] = t.y (); p[2] = t.z (); p[3] = q.x (); p[4] = q.y (); p[5] = q.z ();
warp.setParam (p);
PointXYZ pin (1, 2, 3), pout;
warp.warpPoint (pin, pout);
EXPECT_NEAR (pout.x, 4.15963f, 1e-5);
EXPECT_NEAR (pout.y, -1.51363f, 1e-5);
EXPECT_NEAR (pout.z, 0.922648f, 1e-5);
}
bool
psmove_alignment_quaternion_between_vector_frames(
const Eigen::Vector3f* from[2], const Eigen::Vector3f* to[2], const float tolerance, const Eigen::Quaternionf &initial_q,
Eigen::Quaternionf &out_q)
{
bool success = true;
Eigen::Quaternionf previous_q = initial_q;
Eigen::Quaternionf q = initial_q;
//const float tolerance_squared = tolerance*tolerance; //TODO: This variable is unused, but it should be. Need to re-test with this added since the below test should be: error_squared > tolerance_squared
const int k_max_iterations = 32;
float previous_error_squared = k_real_max;
float error_squared = k_real_max;
float squared_error_delta = k_real_max;
float gamma = 0.5f;
bool backtracked = false;
for (int iteration = 0;
iteration < k_max_iterations && // Haven't exceeded iteration limit
error_squared > tolerance && // Aren't within tolerance of the destination
squared_error_delta > k_normal_epsilon && // Haven't reached a minima
gamma > k_normal_epsilon; // Haven't reduced our step size to zero
iteration++)
{
// Fill in the 6x1 objective function matrix |f_0|
// |f_1|
float error_squared0, error_squared1;
Eigen::Matrix<float, 3, 1> f_0;
psmove_alignment_compute_objective_vector(q, *from[0], *to[0], f_0, &error_squared0);
Eigen::Matrix<float, 3, 1> f_1;
psmove_alignment_compute_objective_vector(q, *from[1], *to[1], f_1, &error_squared1);
Eigen::Matrix<float, 6, 1> f;
f.block<3, 1>(0, 0) = f_0;
f.block<3, 1>(3, 0) = f_1;
error_squared = error_squared0 + error_squared1;
// Make sure this new step hasn't made the error worse
if (error_squared <= previous_error_squared)
{
// We won't have a valid error derivative if we had to back track
squared_error_delta = !backtracked ? fabsf(error_squared - previous_error_squared) : squared_error_delta;
backtracked = false;
// This is a good step.
// Remember it in case the next one makes things worse
previous_error_squared = error_squared;
previous_q = q;
// Fill in the 4x6 objective function Jacobian matrix: [J_0|J_1]
Eigen::Matrix<float, 4, 3> J_0;
psmove_alignment_compute_objective_jacobian(q, *from[0], J_0);
Eigen::Matrix<float, 4, 3> J_1;
psmove_alignment_compute_objective_jacobian(q, *from[1], J_1);
Eigen::Matrix<float, 4, 6> J;
J.block<4, 3>(0, 0) = J_0; J.block<4, 3>(0, 3) = J_1;
// Compute the gradient of the objective function
Eigen::Matrix<float, 4, 1> gradient_f = J*f;
Eigen::Quaternionf gradient_q =
Eigen::Quaternionf(gradient_f(0, 0), gradient_f(1, 0), gradient_f(2, 0), gradient_f(3, 0));
// The gradient points toward the maximum, so we subtract it off to head toward the minimum.
// The step scale 'gamma' is just a guess.
q = Eigen::Quaternionf(q.coeffs() - gradient_q.coeffs()*gamma); //q-= gradient_q*gamma;
q.normalize();
}
else
{
// The step made the error worse.
// Return to the previous orientation and half our step size.
q = previous_q;
gamma /= 2.f;
backtracked = true;
}
}
if (error_squared > tolerance)
{
// Make sure we didn't fail to converge on the goal
success = false;
}
out_q= q;
return success;
}
/**
* @brief Callback for feedback subscriber for getting the transformation of moved markers
*
* @param feedback subscribed from geometry_map/map/feedback
*/
void ShapeVisualization::setShapePosition(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)//,const cob_3d_mapping_msgs::Shape& shape)
{
cob_3d_mapping_msgs::ShapeArray map_msg;
map_msg.header.frame_id="/map";
map_msg.header.stamp = ros::Time::now();
int shape_id,index;
index=-1;
stringstream name(feedback->marker_name);
Eigen::Quaternionf quat;
Eigen::Matrix3f rotationMat;
Eigen::MatrixXf rotationMatInit;
Eigen::Vector3f normalVec;
Eigen::Vector3f normalVecNew;
Eigen::Vector3f newCentroid;
Eigen::Matrix4f transSecondStep;
if (feedback->marker_name != "Text"){
name >> shape_id ;
cob_3d_mapping::Polygon p;
for(int i=0;i<sha.shapes.size();++i)
{
if (sha.shapes[i].id == shape_id)
{
index = i;
}
}
// temporary fix.
//do nothing if index of shape is not found
// this is not supposed to occur , but apparently it does
if(index==-1){
ROS_WARN("shape not in map array");
return;
}
cob_3d_mapping::fromROSMsg (sha.shapes.at(index), p);
if (feedback->event_type == 2 && feedback->menu_entry_id == 5){
quatInit.x() = (float)feedback->pose.orientation.x ; //normalized
quatInit.y() = (float)feedback->pose.orientation.y ;
quatInit.z() = (float)feedback->pose.orientation.z ;
quatInit.w() = (float)feedback->pose.orientation.w ;
oldCentroid (0) = (float)feedback->pose.position.x ;
oldCentroid (1) = (float)feedback->pose.position.y ;
oldCentroid (2) = (float)feedback->pose.position.z ;
quatInit.normalize() ;
rotationMatInit = quatInit.toRotationMatrix() ;
transInit.block(0,0,3,3) << rotationMatInit ;
transInit.col(3).head(3) << oldCentroid(0) , oldCentroid(1), oldCentroid(2) ;
transInit.row(3) << 0,0,0,1 ;
transInitInv = transInit.inverse() ;
Eigen::Affine3f affineInitFinal (transInitInv) ;
affineInit = affineInitFinal ;
std::cout << "transInit : " << "\n" << affineInitFinal.matrix() << "\n" ;
}
if (feedback->event_type == 5){
/* the name of the marker is arrows_shape_.id, we need to erase the "arrows_" part */
//string strName(feedback->marker_name);
//strName.erase(strName.begin(),strName.begin()+7);
// stringstream name(strName);
stringstream name(feedback->marker_name);
name >> shape_id ;
cob_3d_mapping::Polygon p;
cob_3d_mapping::fromROSMsg (sha.shapes.at(index), p);
quat.x() = (float)feedback->pose.orientation.x ; //normalized
quat.y() = (float)feedback->pose.orientation.y ;
quat.z() = (float)feedback->pose.orientation.z ;
quat.w() = (float)feedback->pose.orientation.w ;
quat.normalize() ;
rotationMat = quat.toRotationMatrix() ;
normalVec << sha.shapes.at(index).params[0], //normalized
sha.shapes.at(index).params[1],
sha.shapes.at(index).params[2];
newCentroid << (float)feedback->pose.position.x ,
(float)feedback->pose.position.y ,
(float)feedback->pose.position.z ;
transSecondStep.block(0,0,3,3) << rotationMat ;
transSecondStep.col(3).head(3) << newCentroid(0) , newCentroid(1), newCentroid(2) ;
transSecondStep.row(3) << 0,0,0,1 ;
Eigen::Affine3f affineSecondStep(transSecondStep) ;
std::cout << "transfrom : " << "\n" << affineSecondStep.matrix() << "\n" ;
Eigen::Affine3f affineFinal(affineSecondStep*affineInit) ;
Eigen::Matrix4f matFinal = (transSecondStep*transInitInv) ;
normalVecNew = (matFinal.block(0,0,3,3))* normalVec;
// newCentroid = transFinal *OldCentroid ;
sha.shapes.at(index).centroid.x = newCentroid(0) ;
sha.shapes.at(index).centroid.y = newCentroid(1) ;
sha.shapes.at(index).centroid.z = newCentroid(2) ;
sha.shapes.at(index).params[0] = normalVecNew(0) ;
sha.shapes.at(index).params[1] = normalVecNew(1) ;
sha.shapes.at(index).params[2] = normalVecNew(2) ;
std::cout << "transfromFinal : " << "\n" << affineFinal.matrix() << "\n" ;
pcl::PointCloud<pcl::PointXYZ> pc;
pcl::PointXYZ pt;
sensor_msgs::PointCloud2 pc2;
for(unsigned int j=0; j<p.contours.size(); j++)
{
for(unsigned int k=0; k<p.contours[j].size(); k++)
{
p.contours[j][k] = affineFinal * p.contours[j][k];
pt.x = p.contours[j][k][0] ;
pt.y = p.contours[j][k][1] ;
pt.z = p.contours[j][k][2] ;
pc.push_back(pt) ;
}
}
pcl::toROSMsg (pc, pc2);
sha.shapes.at(index).points.clear() ;
sha.shapes.at(index).points.push_back (pc2);
// uncomment when using test_shape_array
// for(unsigned int i=0;i<sha.shapes.size();i++){
// map_msg.header = sha.shapes.at(i).header ;
// map_msg.shapes.push_back(sha.shapes.at(i)) ;
// }
// shape_pub_.publish(map_msg);
// end uncomment
modified_shapes_.shapes.push_back(sha.shapes.at(index));
}
int main(int argc, char** argv) {
parseCommandLine(argc, argv);
/* CARICO GLI ARGOMENTI*/
/* CREO L'OGGETTO CHE CALCOLA LE CORRISPONDENZE*/
Correspondance corr;
/* LEGGO LE POINT CLOUD*/
if (pcl::io::loadPCDFile<point> (modstr, *(corr.Modello)) == -1) //* load the file
{
PCL_ERROR("Couldn't read file \n");
return (-1);
}
if (pcl::io::loadPCDFile<point> (scestr, *(corr.Scena)) == -1) //* load the file
{
PCL_ERROR("Couldn't read file \n");
return (-1);
}
if (transformscene) {
Eigen::Affine3f T;
Eigen::Quaternionf q = Eigen::Quaternionf(qw, qx, qy, qz);
q.normalize();
T = Eigen::Translation<float, 3>(tx, ty, tz) * q;
CloudT old(new pcl::PointCloud<point>);
pcl::copyPointCloud(*corr.Scena, *old);
pcl::transformPointCloud(*corr.Scena, *corr.Scena, T);
pcl::visualization::PointCloudColorHandlerCustom<point> white(old, 255, 255, 255);
pcl::visualization::PointCloudColorHandlerCustom<point> green(corr.Scena, 0, 255, 0);
pcl::visualization::PCLVisualizer tv;
tv.addPointCloud<point>(old, white, "old");
tv.addPointCloud<point>(corr.Scena, green, "new");
while (!tv.wasStopped()) {
tv.spinOnce(100);
}
tv.setSize(1, 1);
}
/* VISUALIZZO LE POINTCLOUD*/
pcl::visualization::PCLVisualizer::Ptr v(new pcl::visualization::PCLVisualizer);
pcl::visualization::PCLVisualizer::Ptr w(new pcl::visualization::PCLVisualizer);
pcl::visualization::PointCloudColorHandlerCustom<point> whiteM(corr.Modello, 255, 255, 255);
pcl::visualization::PointCloudColorHandlerCustom<point> whiteS(corr.Scena, 255, 255, 255);
v->addPointCloud<point>(corr.Modello, whiteM, "modello");
v->setPosition(10, 10);
v->setSize(640, 480);
w->addPointCloud<point>(corr.Scena, whiteS, "scena");
w->setPosition(650, 10);
w->setSize(640, 480);
v->setWindowName("MODELLO");
w->setWindowName("SCENA");
while (!(v->wasStopped())&&!(w->wasStopped())) {
v->spinOnce(10);
w->spinOnce(10);
};
w->setSize(1, 1);
v->setSize(1, 1);
corr.kmatch = kmatchglobal;
corr.HistDistTreshSquare = HistDistTreshSquare;
corr.DescrizioneModello.name="Modello";
corr.DescrizioneScena.name="Scene";
/* FILTRAGGIO E ELIMINAZIONE DEL PIANO */
corr.DescrizioneModello.ep.rimuovipiano = modelremoveplane;
corr.DescrizioneModello.ep.percent = PlanePercent;
corr.DescrizioneModello.ep.distance_threshold = distance_treshold;
corr.DescrizioneModello.ep.max_iterations = PlaneMaxIter;
corr.DescrizioneModello.ep.Rfilter = Rfilter;
corr.DescrizioneScena.ep.rimuovipiano =sceneremoveplane;
corr.DescrizioneScena.ep.percent = PlanePercent;
corr.DescrizioneScena.ep.distance_threshold = distance_treshold;
corr.DescrizioneScena.ep.max_iterations = PlaneMaxIter;
corr.DescrizioneScena.ep.Rfilter = Rfilter;
/* DESCRIZIONE*/
corr.DescrizioneModello.fpfh.RNormal = RNormal;
corr.DescrizioneModello.sc.color_importance = color_importance;
corr.DescrizioneModello.sc.normal_importance = normal_importance;
corr.DescrizioneModello.sc.spatial_importance = spatial_importance;
corr.DescrizioneModello.sc.seed_resolution = seed_resolution;
corr.DescrizioneModello.sc.voxel_resolution = voxel_resolution;
corr.DescrizioneScena.fpfh.RNormal = RNormal;
corr.DescrizioneScena.sc.color_importance = color_importance;
corr.DescrizioneScena.sc.normal_importance = normal_importance;
corr.DescrizioneScena.sc.spatial_importance = spatial_importance;
corr.DescrizioneScena.sc.seed_resolution = seed_resolution;
corr.DescrizioneScena.sc.voxel_resolution = voxel_resolution;
corr.compute();
corr.visualizza();
corr.DescrizioneModello.fpfh.SalvaDatiPatchCSV();
corr.DescrizioneScena.fpfh.SalvaDatiPatchCSV();
return 0;
}