template <typename PointT> void
pcl::SampleConsensusModelRegistration2D<PointT>::selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
{
if (indices_->size () != indices_tgt_->size ())
{
PCL_ERROR ("[pcl::SampleConsensusModelRegistration2D::selectWithinDistance] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ());
inliers.clear ();
return;
}
if (!target_)
{
PCL_ERROR ("[pcl::SampleConsensusModelRegistration2D::selectWithinDistance] No target dataset given!\n");
return;
}
double thresh = threshold * threshold;
int nr_p = 0;
inliers.resize (indices_->size ());
error_sqr_dists_.resize (indices_->size ());
Eigen::Matrix4f transform;
transform.row (0).matrix () = model_coefficients.segment<4>(0);
transform.row (1).matrix () = model_coefficients.segment<4>(4);
transform.row (2).matrix () = model_coefficients.segment<4>(8);
transform.row (3).matrix () = model_coefficients.segment<4>(12);
for (size_t i = 0; i < indices_->size (); ++i)
{
Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x,
input_->points[(*indices_)[i]].y,
input_->points[(*indices_)[i]].z, 1);
Eigen::Vector4f p_tr (transform * pt_src);
// Project the point on the image plane
Eigen::Vector3f p_tr3 (p_tr[0], p_tr[1], p_tr[2]);
Eigen::Vector3f uv (projection_matrix_ * p_tr3);
if (uv[2] < 0)
continue;
uv /= uv[2];
double distance = ((uv[0] - target_->points[(*indices_tgt_)[i]].u) *
(uv[0] - target_->points[(*indices_tgt_)[i]].u) +
(uv[1] - target_->points[(*indices_tgt_)[i]].v) *
(uv[1] - target_->points[(*indices_tgt_)[i]].v));
// Calculate the distance from the transformed point to its correspondence
if (distance < thresh)
{
inliers[nr_p] = (*indices_)[i];
error_sqr_dists_[nr_p] = distance;
++nr_p;
}
}
inliers.resize (nr_p);
error_sqr_dists_.resize (nr_p);
}
template <typename PointT> void
pcl::SampleConsensusModelRegistration2D<PointT>::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
{
PCL_INFO ("[pcl::SampleConsensusModelRegistration2D<PointT>::getDistancesToModel]\n");
if (indices_->size () != indices_tgt_->size ())
{
PCL_ERROR ("[pcl::SampleConsensusModelRegistration2D::getDistancesToModel] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ());
distances.clear ();
return;
}
if (!target_)
{
PCL_ERROR ("[pcl::SampleConsensusModelRegistration2D::getDistanceToModel] No target dataset given!\n");
return;
}
distances.resize (indices_->size ());
// Get the 4x4 transformation
Eigen::Matrix4f transform;
transform.row (0).matrix () = model_coefficients.segment<4>(0);
transform.row (1).matrix () = model_coefficients.segment<4>(4);
transform.row (2).matrix () = model_coefficients.segment<4>(8);
transform.row (3).matrix () = model_coefficients.segment<4>(12);
for (size_t i = 0; i < indices_->size (); ++i)
{
Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x,
input_->points[(*indices_)[i]].y,
input_->points[(*indices_)[i]].z, 1);
Eigen::Vector4f p_tr (transform * pt_src);
// Project the point on the image plane
Eigen::Vector3f p_tr3 (p_tr[0], p_tr[1], p_tr[2]);
Eigen::Vector3f uv (projection_matrix_ * p_tr3);
if (uv[2] < 0)
continue;
uv /= uv[2];
// Calculate the distance from the transformed point to its correspondence
// need to compute the real norm here to keep MSAC and friends general
distances[i] = std::sqrt ((uv[0] - target_->points[(*indices_tgt_)[i]].u) *
(uv[0] - target_->points[(*indices_tgt_)[i]].u) +
(uv[1] - target_->points[(*indices_tgt_)[i]].v) *
(uv[1] - target_->points[(*indices_tgt_)[i]].v));
}
}
template <typename PointT> void
pcl::SampleConsensusModelRegistration<PointT>::selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
{
if (indices_->size () != indices_tgt_->size ())
{
PCL_ERROR ("[pcl::SampleConsensusModelRegistration::selectWithinDistance] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ());
inliers.clear ();
return;
}
if (!target_)
{
PCL_ERROR ("[pcl::SampleConsensusModelRegistration::selectWithinDistance] No target dataset given!\n");
return;
}
double thresh = threshold * threshold;
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
{
inliers.clear ();
return;
}
inliers.resize (indices_->size ());
Eigen::Matrix4f transform;
transform.row (0) = model_coefficients.segment<4>(0);
transform.row (1) = model_coefficients.segment<4>(4);
transform.row (2) = model_coefficients.segment<4>(8);
transform.row (3) = model_coefficients.segment<4>(12);
int nr_p = 0;
for (size_t i = 0; i < indices_->size (); ++i)
{
Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x,
input_->points[(*indices_)[i]].y,
input_->points[(*indices_)[i]].z, 1);
Eigen::Vector4f pt_tgt (target_->points[(*indices_tgt_)[i]].x,
target_->points[(*indices_tgt_)[i]].y,
target_->points[(*indices_tgt_)[i]].z, 1);
Eigen::Vector4f p_tr (transform * pt_src);
// Calculate the distance from the transformed point to its correspondence
if ((p_tr - pt_tgt).squaredNorm () < thresh)
inliers[nr_p++] = (*indices_)[i];
}
inliers.resize (nr_p);
}
template <typename PointT> void
pcl::SampleConsensusModelRegistration<PointT>::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
{
if (indices_->size () != indices_tgt_->size ())
{
PCL_ERROR ("[pcl::SampleConsensusModelRegistration::getDistancesToModel] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ());
distances.clear ();
return;
}
if (!target_)
{
PCL_ERROR ("[pcl::SampleConsensusModelRegistration::getDistanceToModel] No target dataset given!\n");
return;
}
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
{
distances.clear ();
return;
}
distances.resize (indices_->size ());
// Get the 4x4 transformation
Eigen::Matrix4f transform;
transform.row (0) = model_coefficients.segment<4>(0);
transform.row (1) = model_coefficients.segment<4>(4);
transform.row (2) = model_coefficients.segment<4>(8);
transform.row (3) = model_coefficients.segment<4>(12);
for (size_t i = 0; i < indices_->size (); ++i)
{
Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x,
input_->points[(*indices_)[i]].y,
input_->points[(*indices_)[i]].z, 1);
Eigen::Vector4f pt_tgt (target_->points[(*indices_tgt_)[i]].x,
target_->points[(*indices_tgt_)[i]].y,
target_->points[(*indices_tgt_)[i]].z, 1);
Eigen::Vector4f p_tr (transform * pt_src);
// Calculate the distance from the transformed point to its correspondence
// need to compute the real norm here to keep MSAC and friends general
distances[i] = (p_tr - pt_tgt).norm ();
}
}
Eigen::Matrix4f ConsistencyTest::initPose2D( std::map<unsigned, unsigned> &matched_planes )
{
//Calculate rotation
Matrix3f normalCovariances = Matrix3f::Zero();
for(map<unsigned, unsigned>::iterator it = matched_planes.begin(); it != matched_planes.end(); it++)
normalCovariances += PBMTarget.vPlanes[it->second].v3normal * PBMSource.vPlanes[it->first].v3normal.transpose();
normalCovariances(1,1) += 100; // Rotation "restricted" to the y axis
JacobiSVD<MatrixXf> svd(normalCovariances, ComputeThinU | ComputeThinV);
Matrix3f Rotation = svd.matrixU() * svd.matrixV().transpose();
if(Rotation.determinant() < 0)
// Rotation.row(2) *= -1;
Rotation = -Rotation;
// Calculate translation
Vector3f translation;
Vector3f center_data = Vector3f::Zero(), center_model = Vector3f::Zero();
Vector3f centerFull_data = Vector3f::Zero(), centerFull_model = Vector3f::Zero();
unsigned numFull = 0, numNonStruct = 0;
for(map<unsigned, unsigned>::iterator it = matched_planes.begin(); it != matched_planes.end(); it++)
{
if(PBMSource.vPlanes[it->first].bFromStructure) // The certainty in center of structural planes is too low
continue;
++numNonStruct;
center_data += PBMSource.vPlanes[it->first].v3center;
center_model += PBMTarget.vPlanes[it->second].v3center;
if(PBMSource.vPlanes[it->first].bFullExtent)
{
centerFull_data += PBMSource.vPlanes[it->first].v3center;
centerFull_model += PBMTarget.vPlanes[it->second].v3center;
++numFull;
}
}
if(numFull > 0)
{
translation = (-centerFull_model + Rotation * centerFull_data) / numFull;
}
else
{
translation = (-center_model + Rotation * center_data) / numNonStruct;
}
translation[1] = 0; // Restrict no translation in the y axis
// Form SE3 transformation matrix. This matrix maps the model into the current data reference frame
Eigen::Matrix4f rigidTransf;
rigidTransf.block(0,0,3,3) = Rotation;
rigidTransf.block(0,3,3,1) = translation;
rigidTransf.row(3) << 0,0,0,1;
return rigidTransf;
}
Eigen::Matrix4f computeGroundTruth(const std::string path_groundtruth, const int id)
{
std::string line;
std::ifstream file(path_groundtruth);
int offset = 3;
std::vector<std::string> splitVec;
int index = id * 5 + offset;
Eigen::Matrix4f transformationMatrix;
if (file.is_open())
{
int count = 1;
bool done = false;
while (count<index)
{
std::getline(file, line);
count++;
}
//start reading matrix from file
for (int i = 0; i<4; i++)
{
std::getline(file, line);
boost::split(splitVec, line, boost::is_any_of("\t"), boost::token_compress_on);
transformationMatrix.row(i) << boost::lexical_cast<float>(splitVec.at(0)),
boost::lexical_cast<float>(splitVec.at(1)),
boost::lexical_cast<float>(splitVec.at(2)),
boost::lexical_cast<float>(splitVec.at(3));
}
}
else
std::cout << "GroundTruth file not found" << std::endl;
transformationMatrix.inverse();
return transformationMatrix;
}
/**
* @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));
}
bool Utils::
isOrthographic(const Eigen::Matrix4f& iMatrix) {
const Eigen::Vector4f unitW(0,0,0,1);
float dot = fabs(iMatrix.row(3).normalized()[3]);
return (fabs(dot-1) < 1e-6);
}