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; }