void DrawableCloud::draw() {
if(_parameter->show() && _cloud) {
glPushMatrix();
glMultMatrixf(_transformation.data());
if(_drawablePoints)
_drawablePoints->draw();
if(_drawableNormals)
_drawableNormals->draw();
if(_drawableCovariances)
_drawableCovariances->draw();
if(_drawableCorrespondences)
_drawableCorrespondences->draw();
glPushMatrix();
Eigen::Isometry3f sensorOffset;
sensorOffset.translation() = Eigen::Vector3f(0.0f, 0.0f, 0.0f);
Eigen::Quaternionf quaternion = Eigen::Quaternionf(-.5f, -0.5f, 0.5f, 0.5f);
sensorOffset.linear() = quaternion.toRotationMatrix();
sensorOffset.matrix().row(3) << 0.0f, 0.0f, 0.0f, 1.0f;
glMultMatrixf(sensorOffset.data());
glColor4f(1,0,0,0.5);
glPopMatrix();
glPopMatrix();
}
}
void generateTF(int trans, Eigen::Quaternionf &R, Eigen::Vector3f &t, Eigen::Matrix3f &mR)
{
float strengthR=(trans%3)*0.05;
trans/=3;
float strengthT=(trans%3)*0.05;
trans/=3;
int temp=trans%8;
Eigen::Vector3f axis;
axis(0) = temp&1?1:0;
axis(1) = temp&2?1:0;
axis(2) = temp&4?1:0;
trans/=8;
Eigen::AngleAxisf aa(strengthR,axis);
R = aa.toRotationMatrix();
mR= R.toRotationMatrix();
R = mR;
t(0) = temp&1?1:0;
if(temp&2) t(0)=-t(0);
t(1) = temp&4?1:0;
if(temp&8) t(1)=-t(1);
t(2) = temp&16?1:0;
if(temp&32) t(2)=-t(2);
trans/=64;
t*=strengthT;
}
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);
}
void CPclView::addCameraMesh(const pcl::PointXYZ& position, const Eigen::Quaternionf& rotation,
const QString& id)
{
pcl::PolygonMesh cameraMesh;
initzializeCameraMeshCloud(&cameraMesh.cloud);
Eigen::Vector3f cameraShapeVertices[CAMERA_SHAPE_N_VERTICES];
cameraShapeVertices[0] = Eigen::Vector3f( 0.f, 0.f, 0.f);
cameraShapeVertices[1] = Eigen::Vector3f(-1.f, -1.f, 1.f);
cameraShapeVertices[2] = Eigen::Vector3f(-1.f, 1.f, 1.f);
cameraShapeVertices[3] = Eigen::Vector3f( 1.f, -1.f, 1.f);
cameraShapeVertices[4] = Eigen::Vector3f( 1.f, 1.f, 1.f);
Eigen::Vector3f cameraBasePos(position.x, position.y, position.z);
Eigen::Quaternionf quat = rotation.normalized();
Eigen::Matrix3f rotationMatrix = quat.toRotationMatrix();
for(int i = 0; i < CAMERA_SHAPE_N_VERTICES; i++)
{
cameraShapeVertices[i] = rotationMatrix * cameraShapeVertices[i];
cameraShapeVertices[i] += cameraBasePos;
for(unsigned int j = 0; j < 3; j++)
{
reinterpret_cast<float*>(cameraMesh.cloud.data.data())[i * 3 + j] = cameraShapeVertices[i][j];
}
}
pcl::Vertices v;
v.vertices.push_back(1);
v.vertices.push_back(2);
v.vertices.push_back(4);
v.vertices.push_back(3);
cameraMesh.polygons.push_back(v);
v.vertices.clear();
v.vertices.push_back(0);
v.vertices.push_back(1);
v.vertices.push_back(3);
v.vertices.push_back(0);
cameraMesh.polygons.push_back(v);
v.vertices.clear();
v.vertices.push_back(0);
v.vertices.push_back(2);
v.vertices.push_back(4);
v.vertices.push_back(0);
cameraMesh.polygons.push_back(v);
mpPclVisualizer->addPolylineFromPolygonMesh(cameraMesh, id.toStdString());
}
void addCoordinateFrame (geometry_msgs::Pose pose) {
cloud_viewer.addCoordinateSystem (0.5);
Eigen::Vector3f t;
t(0) = pose.position.x;
t(1) = pose.position.y;
t(2) = -pose.position.z; // PCL seems to have a strange Z axis
Eigen::Quaternionf q;
q.x() = pose.orientation.x;
q.y() = pose.orientation.y;
q.z() = pose.orientation.z;
q.w() = pose.orientation.w;
Eigen::Matrix3f R = q.toRotationMatrix();
Eigen::Vector3f r = R.eulerAngles(0, 1, 2);
Eigen::Affine3f A;
pcl::getTransformation (t(0), t(1), t(2), r(0), r(1), r(2), A);
cloud_viewer.addCoordinateSystem (0.2, A);
}
void ImageMessageListener::cameraInfoCallback(const sensor_msgs::CameraInfoConstPtr& msg) {
cerr << "got camera matrix for topic: " << _camera_info_topic << endl;
if (_listener) {
char buffer[1024];
buffer[0] = 0;
tf::StampedTransform transform;
try{
_listener->waitForTransform(_base_link_frame_id, msg->header.frame_id,
msg->header.stamp,
ros::Duration(0.5) );
_listener->lookupTransform (_base_link_frame_id, msg->header.frame_id,
msg->header.stamp,
transform);
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
return;
}
Eigen::Quaternionf q;
q.x() = transform.getRotation().x();
q.y() = transform.getRotation().y();
q.z() = transform.getRotation().z();
q.w() = transform.getRotation().w();
_camera_offset.linear()=q.toRotationMatrix();
_camera_offset.translation()=Eigen::Vector3f(transform.getOrigin().x(),
transform.getOrigin().y(),
transform.getOrigin().z());
}
int i;
for (int r=0; r<3; r++)
for (int c=0; c<3; c++, i++)
_K(r,c) = msg->K[i];
_K.row(2) << 0,0,1;
_has_camera_matrix = true;
_camera_info_subscriber.shutdown();
}
/**
* @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 LoadPCD::compute()
{
//for each selected filename
for (int k = 0; k < m_filenames.size(); ++k)
{
Eigen::Vector4f origin;
Eigen::Quaternionf orientation;
QString filename = m_filenames[k];
boost::shared_ptr<PCLCloud> cloud_ptr_in = loadSensorMessage(filename, origin, orientation);
if (!cloud_ptr_in) //loading failed?
return 0;
PCLCloud::Ptr cloud_ptr;
if (!cloud_ptr_in->is_dense) //data may contain nans. Remove them
{
//now we need to remove nans
pcl::PassThrough<PCLCloud> passFilter;
passFilter.setInputCloud(cloud_ptr_in);
cloud_ptr = PCLCloud::Ptr(new PCLCloud);
passFilter.filter(*cloud_ptr);
}
else
{
cloud_ptr = cloud_ptr_in;
}
//now we construct a ccGBLSensor with these characteristics
ccGBLSensor * sensor = new ccGBLSensor;
// get orientation as rot matrix
Eigen::Matrix3f eigrot = orientation.toRotationMatrix();
// and copy it into a ccGLMatrix
ccGLMatrix ccRot;
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 3; ++j)
{
ccRot.getColumn(j)[i] = eigrot(i,j);
}
}
ccRot.getColumn(3)[3] = 1.0;
// now in a format good for CloudComapre
//ccGLMatrix ccRot = ccGLMatrix::FromQuaternion(orientation.coeffs().data());
//ccRot = ccRot.transposed();
ccRot.setTranslation(origin.data());
sensor->setRigidTransformation(ccRot);
sensor->setDeltaPhi(static_cast<PointCoordinateType>(0.05));
sensor->setDeltaTheta(static_cast<PointCoordinateType>(0.05));
sensor->setVisible(true);
//uncertainty to some default
sensor->setUncertainty(static_cast<PointCoordinateType>(0.01));
ccPointCloud* out_cloud = sm2ccConverter(cloud_ptr).getCloud();
if (!out_cloud)
return -31;
sensor->setGraphicScale(out_cloud->getBB().getDiagNorm() / 10);
//do the projection on sensor
ccGenericPointCloud* cloud = ccHObjectCaster::ToGenericPointCloud(out_cloud);
int errorCode;
CCLib::SimpleCloud* projectedCloud = sensor->project(cloud,errorCode,true);
if (projectedCloud)
{
//DGM: we don't use it but we still have to delete it!
delete projectedCloud;
projectedCloud = 0;
}
QString cloud_name = QFileInfo(filename).baseName();
out_cloud->setName(cloud_name);
QFileInfo fi(filename);
QString containerName = QString("%1 (%2)").arg(fi.fileName()).arg(fi.absolutePath());
ccHObject* cloudContainer = new ccHObject(containerName);
out_cloud->addChild(sensor);
cloudContainer->addChild(out_cloud);
emit newEntity(cloudContainer);
}
return 1;
}
void ImageMessageListener::imageCallback(const sensor_msgs::ImageConstPtr& in_msg) {
static int counter = 0; // counter to add periodic line breaks
_image_deque.push_back(*in_msg);
if (_image_deque.size()<_deque_length){
return;
}
sensor_msgs::Image msg = _image_deque.front();
_image_deque.pop_front();
if (_listener) {
tf::StampedTransform transform;
try{
_listener->waitForTransform(_odom_frame_id,
_base_link_frame_id,
msg.header.stamp,
ros::Duration(0.5) );
_listener->lookupTransform (_odom_frame_id,
_base_link_frame_id,
msg.header.stamp,
transform);
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}
Eigen::Quaternionf q;
q.x() = transform.getRotation().x();
q.y() = transform.getRotation().y();
q.z() = transform.getRotation().z();
q.w() = transform.getRotation().w();
_odom_pose.linear()=q.toRotationMatrix();
_odom_pose.translation()=Eigen::Vector3f(transform.getOrigin().x(),
transform.getOrigin().y(),
transform.getOrigin().z());
}
cv_bridge::CvImageConstPtr bridge;
bridge = cv_bridge::toCvCopy(msg,msg.encoding);
if (! _has_camera_matrix){
cerr << "received: " << _image_topic << " waiting for camera matrix" << endl;
return;
}
_cv_image = bridge->image.clone();
PinholeImageMessage* img_msg = new PinholeImageMessage(_image_topic, msg.header.frame_id, msg.header.seq, msg.header.stamp.toSec());
img_msg->setImage(_cv_image);
img_msg->setOffset(_camera_offset);
img_msg->setCameraMatrix(_K);
img_msg->setDepthScale(_depth_scale);
if (_imu_interpolator && _listener) {
Eigen::Isometry3f imu = Eigen::Isometry3f::Identity();
Eigen::Quaternionf q_imu;
if (!_imu_interpolator->getOrientation(q_imu, msg.header.stamp.toSec()))
return;
imu.linear() = q_imu.toRotationMatrix();
if (_verbose) {
cerr << "i";
counter++;
if( counter % 1024 == 0 )
cerr << endl;
}
img_msg->setImu(imu);
} else if (_listener) {
img_msg->setOdometry(_odom_pose);
if (_verbose)
cerr << "o";
counter++;
if( counter % 1024 == 0 )
cerr << endl;
} else {
if (_verbose)
cerr << "x";
counter++;
if( counter % 1024 == 0 )
cerr << endl;
}
_sorter->insertMessage(img_msg);
}
int main(int argc, char **argv)
{
std::vector<int> p_file_indices_conf = pcl::console::parse_file_extension_argument (argc, argv, ".conf");
std::string output_directory = ".";
pcl::console::parse_argument (argc, argv, "--directory", output_directory);
for (int i = 0; i < p_file_indices_conf.size(); ++i)
{
QFile conf_file(argv[p_file_indices_conf[i]]);
if(conf_file.open(QIODevice::ReadOnly))
{
QTextStream in(&conf_file);
while(!in.atEnd()) {
QString line = in.readLine();
//qDebug() << line;
QStringList list = line.split(" ");
if (list[0] == "camera" || list[0] == "mesh") continue;
if (list[0] == "bmesh")
{
QString ply_file = list[1];
float tx = list[2].toFloat();
float ty = list[3].toFloat();
float tz = list[4].toFloat();
float qi = list[5].toFloat();
float qj = list[6].toFloat();
float qk = list[7].toFloat();
float ql = list[8].toFloat();
Eigen::Matrix4f transformation = Eigen::Matrix4f::Identity();
Eigen::Vector3f translation = Eigen::Vector3f(tx, ty, tz);
Eigen::Quaternionf quaternion = Eigen::Quaternionf(qi, qj, qk, ql);
transformation.block<3,3>(0,0) = quaternion.toRotationMatrix();
transformation.block<3,1>(0,3) = translation;
QFileInfo fileInfo(ply_file);
CloudIO::exportTransformation(QString(output_directory.c_str()) + "/" + fileInfo.completeBaseName() + ".tf", transformation);
}
}
}
}
// pcl::PLYReader reader;
// pcl::PolygonMesh mesh;
// reader.read("dragonStandRight_0_out.ply", mesh);
// std::cout << mesh.cloud.height << std::endl;
// std::cout << mesh.cloud.width << std::endl;
// QFileInfo info("dragonStandRight_0_out.ply");
// pcl::io::savePLYFile((info.path() + "/" + info.completeBaseName() + "_transformed.ply").toStdString(), mesh);
// pcl::PLYReader reader;
// pcl::PointCloud<pcl::PointXYZRGBNormal> cloud;
// reader.read("bun000.ply", cloud);
// std::cout << cloud << std::endl;
// // pcl::PLYWriter writer;
// // writer.write("bun000_out_out.ply", cloud);
// pcl::PCDWriter writer;
// writer.write("bun000.pcd", cloud);
// pcl::PLYReader reader;
// pcl::PolygonMesh mesh;
// reader.read("bun000_meshlab.ply", mesh);
// std::cout << mesh.cloud.height << std::endl;
// std::cout << mesh.cloud.width << std::endl;
// pcl::visualization::PCLVisualizer visualizer;
// visualizer.addPolygonMesh(mesh);
// visualizer.spin();
// std::cout << mesh.polygons.size() << std::endl;
// for (int i = 0; i < 10; ++i)
// {
// std::cout << mesh.polygons[i].vertices.size() << std::endl;
// for (int j = 0; j < mesh.polygons[i].vertices.size(); ++j)
// {
// std::cout << mesh.polygons[i].vertices[j] << " ";
// }
// std::cout << std::endl;
// }
// for (int i = 0; i < p_file_indices_conf.size(); ++i)
// {
// CloudDataPtr cloudData(new CloudData);
// Eigen::Matrix4f transformation;
// BoundariesPtr boundaries(new Boundaries);
// QString fileName = QString(argv[p_file_indices_ply[i]]);
// CloudIO::importCloudData(fileName, cloudData);
// CloudIO::importTransformation(fileName, transformation);
// CloudIO::importBoundaries(fileName, boundaries);
// Cloud* cloud = cloudManager->addCloud(cloudData, Cloud::fromIO, fileName, transformation);
// cloud->setBoundaries(boundaries);
// }
// float angleThreshold = 10.0f;
// pcl::console::parse_argument (argc, argv, "--angle", angleThreshold);
// angleThreshold = angleThreshold / 180 * M_PI;
// float distanceThreshold = 0.01f;
// pcl::console::parse_argument (argc, argv, "--distance", distanceThreshold);
// QList<Cloud*> cloudList = cloudManager->getAllClouds();
// for (int i = 0; i < cloudList.size(); ++i)
// {
// Cloud *cloud = cloudList[i];
// Eigen::Matrix4f transformation = cloud->getTransformation();
// Eigen::Matrix4f randomRigidTransf = randomRigidTransformation(angleThreshold, distanceThreshold);
// cloud->setTransformation(randomRigidTransf * transformation);
// std::cout << randomRigidTransf << std::endl;
// }
// for (int i = 0; i < cloudList.size(); ++i)
// {
// Cloud *cloud = cloudList[i];
// QString cloudName = cloud->getCloudName();
// CloudDataPtr cloudData = cloud->getCloudData();
// Eigen::Matrix4f transformation = cloud->getTransformation();
// BoundariesPtr boundaries = cloud->getBoundaries();
// QString fileName = cloud->getFileName();
// QFileInfo fileInfo(fileName);
// QString newFileName = fileInfo.path() + "/" + fileInfo.baseName() + "_out.ply";
// CloudIO::exportCloudData(newFileName, cloudData);
// CloudIO::exportTransformation(newFileName, transformation);
// CloudIO::exportBoundaries(newFileName, boundaries);
// CloudIO::exportCloudData(newFileName, cloudData);
// }
return 0;
}