Eigen::Vector3f transformVector(Eigen::Vector3f vector_in, Eigen::Matrix4f transf){
Eigen::Vector4f vector_in_4f(vector_in[0], vector_in[1], vector_in[2], 1);
// Obtener rotación desde matriz de transformación
Eigen::Matrix4f rot;
rot = transf;
rot.col(3) = Eigen::Vector4f(0, 0, 0, 1);
Eigen::Vector4f vector_out_4f = rot*Eigen::Vector4f(vector_in[0], vector_in[1], vector_in[2], 1);
Eigen::Vector3f vector_out (vector_out_4f[0], vector_out_4f[1], vector_out_4f[2]);
return vector_out;
}
Eigen::Matrix4f scale(const Eigen::Matrix4f &m, const Eigen::Vector3f &v) {
Eigen::Matrix4f Result;
Result.col(0) = m.col(0).array() * v(0);
Result.col(1) = m.col(1).array() * v(1);
Result.col(2) = m.col(2).array() * v(2);
Result.col(3) = m.col(3);
return Result;
}
bool PlaneInferredInfo::searchTheFloor(
Eigen::Matrix4f& poseSensor, Plane& plane)
{
if (plane.areaVoxels < 2.0) return false;
// The angle between the Y axis of the camera and the normal of the plane
// will normally be around: a) -1 for the floor b) 1 for the ceiling c) 0
// for the walls
double cosGravityDir = poseSensor.col(1).head(3).dot(plane.v3normal);
if (cosGravityDir < -0.94) // cos 20º = 0.9397
{
double sensorHeight =
(plane.v3normal.dot(poseSensor.col(3).head(3) - plane.v3center));
if (sensorHeight < 0.7 || sensorHeight > 2.0) return false;
mPbMap.FloorPlane = plane.id;
plane.label = mrpt::format("Floor%u", plane.id);
}
else if (cosGravityDir > 0.94)
plane.label = mrpt::format("Ceiling%u", plane.id);
else if (fabs(cosGravityDir) < 0.34)
plane.label = mrpt::format("Wall%u", plane.id);
else
return false;
// For bounding planes (e.g. floor) -> Check that the rest of planes in the
// map are above this one
for (unsigned i = 0; i < mPbMap.vPlanes.size(); i++)
{
if (plane.id == mPbMap.vPlanes[i].id) continue;
if (plane.v3normal.dot(mPbMap.vPlanes[i].v3center - plane.v3center) <
-0.1)
{
plane.label = "";
return false;
}
}
plane.bFromStructure = true;
return true;
}
void transform(pcl::PointCloud<pcl::PointXYZ> &pc, Eigen::Quaternionf &R, Eigen::Vector3f &t)
{
Eigen::Matrix4f m = Eigen::Matrix4f::Identity();
for(int i=0; i<3; i++)
for(int j=0; j<3; j++)
m(i,j) = R.toRotationMatrix()(i,j);
m.col(3).head<3>() = t;
pcl::transformPointCloud(pc,pc,m);
}
template <typename PointSource, typename PointTarget> void
pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::applyState(Eigen::Matrix4f &t, const Vector6d& x) const
{
// !!! CAUTION Stanford GICP uses the Z Y X euler angles convention
Eigen::Matrix3f R;
R = Eigen::AngleAxisf (static_cast<float> (x[5]), Eigen::Vector3f::UnitZ ())
* Eigen::AngleAxisf (static_cast<float> (x[4]), Eigen::Vector3f::UnitY ())
* Eigen::AngleAxisf (static_cast<float> (x[3]), Eigen::Vector3f::UnitX ());
t.topLeftCorner<3,3> ().matrix () = R * t.topLeftCorner<3,3> ().matrix ();
Eigen::Vector4f T (static_cast<float> (x[0]), static_cast<float> (x[1]), static_cast<float> (x[2]), 0.0f);
t.col (3) += T;
}
void DepthFrame::getRegisteredAndReferencedPointCloud(pcl::PointCloud<pcl::PointXYZ>& cloud)
{
pcl::PointXYZ regReferencePoint = getRegisteredReferencePoint();
// Build a translation matrix to the registered reference the cloud after its own registration
Eigen::Matrix4f E = Eigen::Matrix4f::Identity();
E.col(3) = regReferencePoint.getVector4fMap(); // set translation column
// Apply registration first and then referenciation (right to left order in matrix product)
pcl::PointCloud<pcl::PointXYZ>::Ptr pCloudTmp (new pcl::PointCloud<pcl::PointXYZ>);
getPointCloud(*pCloudTmp);
pcl::transformPointCloud(*pCloudTmp, cloud, E.inverse() * m_T);
}
void OBJCTXT<_DOF6>::findCorrespondences3(const OBJCTXT &ctxt, std::vector<SCOR> &cors,
const DOF6 &tf) {
map_cors_.clear();
Eigen::Matrix4f M = tf.getTF4().inverse();
Eigen::Vector3f t=M.col(3).head<3>();
Eigen::Matrix3f R=M.topLeftCorner(3,3);
const float thr = tf.getRotationVariance()+0.05f;
for(size_t j=0; j<ctxt.objs_.size(); j++)
{
OBJECT obj = *ctxt.objs_[j];
obj.transform(R,t,0,0);
for(size_t i=0; i<objs_.size(); i++)
if( (obj.getData().getBB().preassumption(objs_[i]->getData().getBB())>=std::cos(thr+objs_[i]->getData().getBB().ratio())) &&
obj.intersectsBB(*objs_[i], tf.getRotationVariance(),tf.getTranslationVariance())
&& obj.getData().extensionMatch(objs_[i]->getData(),0.7f,0)
)
{
if(obj.intersectsPts(*objs_[i], tf.getRotationVariance(),tf.getTranslationVariance()) ||
objs_[i]->intersectsPts(obj, tf.getRotationVariance(),tf.getTranslationVariance()))
{
//seccond check
map_cors_[ctxt.objs_[j]].push_back(cors.size());
SCOR c;
c.a = objs_[i];
c.b = ctxt.objs_[j];
cors.push_back(c);
}
}
}
#ifdef DEBUG_
ROS_INFO("found %d correspondences (%d %d)", (int)cors.size(), (int)objs_.size(), (int)ctxt.objs_.size());
for(typename std::vector<SCOR>::iterator it = cors.begin(); it!=cors.end(); it++)
{
Debug::Interface::get().addArrow(it->a->getNearestPoint(),it->b->getNearestPoint(), 0,255,0);
}
#endif
}
geometry_msgs::Pose transformPose(geometry_msgs::Pose pose_in, Eigen::Matrix4f transf){
geometry_msgs::Pose pose_out;
// Obtener rotación desde matriz de transformación
Eigen::Matrix4f rot;
rot = transf;
rot.col(3) = Eigen::Vector4f(0, 0, 0, 1);
// Crear normal desde quaternion
tf::Quaternion tf_q;
tf::quaternionMsgToTF(pose_in.orientation, tf_q);
tf::Vector3 normal(1, 0, 0);
normal = tf::quatRotate(tf_q, normal);
normal.normalize();
// Rotar normal
Eigen::Vector3f normal_vector (normal.x(), normal.y(), normal.z());
Eigen::Vector3f normal_rotated = transformVector(normal_vector, transf);
normal_rotated.normalize();
// Obtener quaternion desde normal rotada
pose_out.orientation = coefsToQuaternionMsg(normal_rotated[0], normal_rotated[1], normal_rotated[2]);
// Transportar posición
pose_out.position = transformPoint(pose_in.position, transf);
return pose_out;
}
Eigen::Matrix4f getTransformation(string frame_ini, string frame_end){
tf::TransformListener tf_listener;
tf::StampedTransform stamped_tf;
ros::Time transform_time = ros::Time(0);
Eigen::Matrix4f transformation = Eigen::Matrix4f::Identity();
if (frame_ini.compare(frame_end) == 0)
return transformation;
try{
ROS_INFO("UTIL: Esperando transformacion disponible...");
if (not tf_listener.waitForTransform(frame_end, frame_ini, transform_time, ros::Duration(1))){
ROS_ERROR("UTIL: Transformacion no pudo ser obtenida antes del timeout (%fs)", 1.0);
return transformation;
}
ROS_INFO("UTIL: Guardando transformacion");
tf_listener.lookupTransform(frame_end, frame_ini, ros::Time(0), stamped_tf);
}
catch (tf::TransformException ex){
ROS_ERROR("UTIL: Excepcion al obtener transformacion: %s", ex.what());
return transformation;
}
// tf::Matrix3x3 rotation = stamped_tf.getBasis();
tf::Matrix3x3 rotation (stamped_tf.getRotation());
tf::Vector3 translation = stamped_tf.getOrigin();
Eigen::Matrix3f upleft3x3;
upleft3x3 << rotation[0][0], rotation[0][1], rotation[0][2],
rotation[1][0], rotation[1][1], rotation[1][2],
rotation[2][0], rotation[2][1], rotation[2][2];
// cout << "Rotation: \n" << upleft3x3 << endl;
// cout << "Translation: \n" << translation[0] << " \t" << translation[1] << " \t" << translation[2] << endl;
transformation.block<3,3>(0,0) = upleft3x3;
transformation.col(3) = Eigen::Vector4f(translation[0], translation[1], translation[2], 1);
// cout << "Todo: \n" << transformation << endl;
return transformation;
/* transformation << rotation[0][0], rotation[0][1], rotation[0][2], translation[0],
rotation[1][0], rotation[1][1], rotation[1][2], translation[1],
rotation[2][0], rotation[2][1], rotation[2][2], translation[2],
0, 0, 0, 1;*/
}
Eigen::Matrix4f translate(const Eigen::Matrix4f &m, const Eigen::Vector3f &v) {
Eigen::Matrix4f Result = m;
Result.col(3) = m.col(0).array() * v(0) + m.col(1).array() * v(1) +
m.col(2).array() * v(2) + m.col(3).array();
return Result;
}
void trackNewCloud(const sensor_msgs::PointCloud2Ptr& msg)
{
ros::Time start_time_stamp = msg->header.stamp;
boost::posix_time::ptime start_time = boost::posix_time::microsec_clock::local_time ();
//std::cout << (start_time - last_cloud_).total_nanoseconds () * 1.0e-9 << std::endl;
float time_ms = (start_time_stamp - last_cloud_ros_time_).toSec() * 1e3;
last_cloud_ = start_time;
last_cloud_ros_time_ = start_time_stamp;
pcl::ScopeTime t("trackNewCloud");
scene_.reset(new pcl::PointCloud<PointT>);
pcl::moveFromROSMsg (*msg, *scene_);
v4r::DataMatrix2D<Eigen::Vector3f> kp_cloud;
cv::Mat_<cv::Vec3b> image;
v4r::convertCloud(*scene_, kp_cloud, image);
int cam_idx=-1;
bool is_ok = camtracker->track(image, kp_cloud, pose_, conf_, cam_idx);
if(debug_image_publisher_.getNumSubscribers())
{
drawConfidenceBar(image, conf_);
sensor_msgs::ImagePtr image_msg = cv_bridge::CvImage(msg->header, "bgr8", image).toImageMsg();
debug_image_publisher_.publish(image_msg);
}
std::cout << time_ms << " conf:" << conf_ << std::endl;
if(is_ok)
{
selectFrames(*scene_, cam_idx, pose_);
tf::Transform transform;
//v4r::invPose(pose, inv_pose);
transform.setOrigin(tf::Vector3(pose_(0,3), pose_(1,3), pose_(2,3)));
tf::Matrix3x3 R(pose_(0,0), pose_(0,1), pose_(0,2),
pose_(1,0), pose_(1,1), pose_(1,2),
pose_(2,0), pose_(2,1), pose_(2,2));
tf::Quaternion q;
R.getRotation(q);
transform.setRotation(q);
ros::Time now_sync = ros::Time::now();
cameraTransformBroadcaster.sendTransform(tf::StampedTransform(transform, now_sync, "camera_rgb_optical_frame", "world"));
bool publish_trajectory = true;
if (!trajectory_marker_.points.empty())
{
const geometry_msgs::Point& last = trajectory_marker_.points.back();
Eigen::Vector3f v_last(last.x, last.y, last.z);
Eigen::Vector3f v_curr(-pose_.col(3).head<3>());
if ((v_last - v_curr).norm() < trajectory_threshold_)
publish_trajectory = false;
}
if (publish_trajectory)
{
geometry_msgs::Point p;
p.x = -pose_(0,3);
p.y = -pose_(1,3);
p.z = -pose_(2,3);
std_msgs::ColorRGBA c;
c.a = 1.0;
c.g = conf_;
trajectory_marker_.points.push_back(p);
trajectory_marker_.colors.push_back(c);
trajectory_marker_.header.stamp = msg->header.stamp;
trajectory_publisher_.publish(trajectory_marker_);
}
}
else
std::cout << "cam tracker not ready!" << std::endl;
/*std_msgs::Float32 conf_mesage;
conf_mesage.data = conf;
confidence_publisher_.publish(conf_mesage);*/
}
/**
* @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));
}
Eigen::Matrix4f Particle::translationnMatrix() const
{
Eigen::Matrix4f mat = Eigen::Matrix4f::Identity();
mat.col(3) = Eigen::Vector4f(m_position.x(), m_position.y(), m_position.z(), 1.0);
return mat;
}