本文整理汇总了C++中eigen::Affine3f类的典型用法代码示例。如果您正苦于以下问题:C++ Affine3f类的具体用法?C++ Affine3f怎么用?C++ Affine3f使用的例子?那么恭喜您, 这里精选的类代码示例或许可以为您提供帮助。
在下文中一共展示了Affine3f类的20个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于我们的系统推荐出更棒的C++代码示例。
示例1: newCube
void
pcl::kinfuLS::WorldModel<PointT>::getExistingData(const double previous_origin_x, const double previous_origin_y, const double previous_origin_z, const double offset_x, const double offset_y, const double offset_z, const double volume_x, const double volume_y, const double volume_z, pcl::PointCloud<PointT> &existing_slice)
{
double newOriginX = previous_origin_x + offset_x;
double newOriginY = previous_origin_y + offset_y;
double newOriginZ = previous_origin_z + offset_z;
double newLimitX = newOriginX + volume_x;
double newLimitY = newOriginY + volume_y;
double newLimitZ = newOriginZ + volume_z;
// filter points in the space of the new cube
PointCloudPtr newCube (new pcl::PointCloud<PointT>);
// condition
ConditionAndPtr range_condAND (new pcl::ConditionAnd<PointT> ());
range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, newOriginX)));
range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, newLimitX)));
range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, newOriginY)));
range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, newLimitY)));
range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, newOriginZ)));
range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, newLimitZ)));
// build the filter
pcl::ConditionalRemoval<PointT> condremAND (true);
condremAND.setCondition (range_condAND);
condremAND.setInputCloud (world_);
condremAND.setKeepOrganized (false);
// apply filter
condremAND.filter (*newCube);
// filter points that belong to the new slice
ConditionOrPtr range_condOR (new pcl::ConditionOr<PointT> ());
if(offset_x >= 0)
range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, previous_origin_x + volume_x - 1.0 )));
else
range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, previous_origin_x )));
if(offset_y >= 0)
range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, previous_origin_y + volume_y - 1.0 )));
else
range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, previous_origin_y )));
if(offset_z >= 0)
range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, previous_origin_z + volume_z - 1.0 )));
else
range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, previous_origin_z )));
// build the filter
pcl::ConditionalRemoval<PointT> condrem (true);
condrem.setCondition (range_condOR);
condrem.setInputCloud (newCube);
condrem.setKeepOrganized (false);
// apply filter
condrem.filter (existing_slice);
if(existing_slice.points.size () != 0)
{
//transform the slice in new cube coordinates
Eigen::Affine3f transformation;
transformation.translation ()[0] = newOriginX;
transformation.translation ()[1] = newOriginY;
transformation.translation ()[2] = newOriginZ;
transformation.linear ().setIdentity ();
transformPointCloud (existing_slice, existing_slice, transformation.inverse ());
}
}
开发者ID:2php,项目名称:pcl,代码行数:70,代码来源:world_model.hpp
示例2: render
void SystemView::render(const Eigen::Affine3f& parentTrans)
{
if(size() == 0)
return;
Eigen::Affine3f trans = getTransform() * parentTrans;
// draw the list elements (titles, backgrounds, logos)
const float logoSizeX = logoSize().x() + LOGO_PADDING;
int logoCount = (int)(mSize.x() / logoSizeX) + 2; // how many logos we need to draw
int center = (int)(mCamOffset);
if(mEntries.size() == 1)
logoCount = 1;
// draw background extras
Eigen::Affine3f extrasTrans = trans;
int extrasCenter = (int)mExtrasCamOffset;
for(int i = extrasCenter - 1; i < extrasCenter + 2; i++)
{
int index = i;
while(index < 0)
index += mEntries.size();
while(index >= (int)mEntries.size())
index -= mEntries.size();
extrasTrans.translation() = trans.translation() + Eigen::Vector3f((i - mExtrasCamOffset) * mSize.x(), 0, 0);
Eigen::Vector2i clipRect = Eigen::Vector2i((int)((i - mExtrasCamOffset) * mSize.x()), 0);
Renderer::pushClipRect(clipRect, mSize.cast<int>());
mEntries.at(index).data.backgroundExtras->render(extrasTrans);
Renderer::popClipRect();
}
// fade extras if necessary
if(mExtrasFadeOpacity)
{
Renderer::setMatrix(trans);
Renderer::drawRect(0.0f, 0.0f, mSize.x(), mSize.y(), 0x00000000 | (unsigned char)(mExtrasFadeOpacity * 255));
}
// draw logos
float xOff = (mSize.x() - logoSize().x())/2 - (mCamOffset * logoSizeX);
float yOff = (mSize.y() - logoSize().y())/2;
// background behind the logos
Renderer::setMatrix(trans);
Renderer::drawRect(0.f, (mSize.y() - BAND_HEIGHT) / 2, mSize.x(), BAND_HEIGHT, 0xFFFFFFD8);
Eigen::Affine3f logoTrans = trans;
for(int i = center - logoCount/2; i < center + logoCount/2 + 1; i++)
{
int index = i;
while(index < 0)
index += mEntries.size();
while(index >= (int)mEntries.size())
index -= mEntries.size();
logoTrans.translation() = trans.translation() + Eigen::Vector3f(i * logoSizeX + xOff, yOff, 0);
if(index == mCursor) //scale our selection up
{
// selected
const std::shared_ptr<GuiComponent>& comp = mEntries.at(index).data.logoSelected;
comp->setOpacity(0xFF);
comp->render(logoTrans);
}else{
// not selected
const std::shared_ptr<GuiComponent>& comp = mEntries.at(index).data.logo;
comp->setOpacity(0x80);
comp->render(logoTrans);
}
}
Renderer::setMatrix(trans);
Renderer::drawRect(mSystemInfo.getPosition().x(), mSystemInfo.getPosition().y() - 1, mSize.x(), mSystemInfo.getSize().y(), 0xDDDDDD00 | (unsigned char)(mSystemInfo.getOpacity() / 255.f * 0xD8));
mSystemInfo.render(trans);
}
开发者ID:HotMykeul,项目名称:EmulationStation,代码行数:79,代码来源:SystemView.cpp
示例3: main
int main(int argc, char** argv)
{
if( argc < 2 )
{
printPrompt( argv[0] );
return -1;
}
initModule_nonfree();
// Get Input Data
ifstream file(argv[1]);
if ( !file.is_open() )
return false;
string str;
// Image Name
getline( file, str ); getline( file, str );
string image_name = str;
// Cloud Name
getline( file, str ); getline( file, str );
string cloud_name = str;
// width of images to be created.
getline( file, str ); getline( file, str );
int w = atoi(str.c_str());
// height of images to be created
getline( file, str ); getline( file, str );
int h = atoi(str.c_str());
// resolution of voxel grids
getline( file, str ); getline( file, str );
float r = atof(str.c_str());
// f (distance from pinhole)
getline( file, str ); getline( file, str );
float f = atof(str.c_str());
// thetax (initial rotation about X Axis of map)
getline( file, str ); getline( file, str );
float thetaX = atof(str.c_str());
// thetay (initial rotation about Y Axis of map)
getline( file, str ); getline( file, str );
float thetaY = atof(str.c_str());
// number of points to go to
getline( file, str ); getline( file, str );
float nop = atoi(str.c_str());
// Number of divisions
getline( file, str ); getline( file, str );
float divs = atoi(str.c_str());
// Number of images to return
getline( file, str ); getline( file, str );
int numtoreturn = atoi(str.c_str());
// Should we load or create photos?
getline( file, str ); getline( file, str );
string lorc =str.c_str();
// Directory to look for photos
getline( file, str ); getline( file, str );
string dir =str.c_str();
// Directory to look for kp and descriptors
getline( file, str ); getline( file, str );
string kdir =str.c_str();
// save photos?
getline( file, str ); getline( file, str );
string savePhotos =str.c_str();
file.close();
// Done Getting Input Data
map<vector<float>, Mat> imagemap;
map<vector<float>, Mat> surfmap;
map<vector<float>, Mat> siftmap;
map<vector<float>, Mat> orbmap;
map<vector<float>, Mat> fastmap;
imagemap.clear();
vector<KeyPoint> SurfKeypoints;
vector<KeyPoint> SiftKeypoints;
vector<KeyPoint> OrbKeypoints;
vector<KeyPoint> FastKeypoints;
Mat SurfDescriptors;
Mat SiftDescriptors;
Mat OrbDescriptors;
Mat FastDescriptors;
int minHessian = 300;
SurfFeatureDetector SurfDetector (minHessian);
SiftFeatureDetector SiftDetector (minHessian);
OrbFeatureDetector OrbDetector (minHessian);
FastFeatureDetector FastDetector (minHessian);
SurfDescriptorExtractor SurfExtractor;
SiftDescriptorExtractor SiftExtractor;
OrbDescriptorExtractor OrbExtractor;
if ( !fs::exists( dir ) || lorc == "c" )
{ // Load Point Cloud and render images
PointCloud<PT>::Ptr cloud (new pcl::PointCloud<PT>);
io::loadPCDFile<PT>(cloud_name, *cloud);
Eigen::Affine3f tf = Eigen::Affine3f::Identity();
//.........这里部分代码省略.........
开发者ID:aarich,项目名称:localize-with-map,代码行数:101,代码来源:createDBWithPointCloud.cpp
示例4: simulate_callback
void simulate_callback (const pcl::visualization::KeyboardEvent &event,
void* viewer_void)
{
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer = *static_cast<boost::shared_ptr<pcl::visualization::PCLVisualizer> *> (viewer_void);
// I choose v for virtual as s for simulate is takwen
if (event.getKeySym () == "v" && event.keyDown ())
{
std::cout << "v was pressed => simulate cloud" << std::endl;
std::vector<pcl::visualization::Camera> cams;
viewer->getCameras(cams);
if (cams.size() !=1){
std::cout << "n cams not 1 exiting\n"; // for now in case ...
return;
}
// cout << "n cams: " << cams.size() << "\n";
pcl::visualization::Camera cam = cams[0];
Eigen::Affine3f pose;
pose = viewer->getViewerPose() ;
std::cout << cam.pos[0] << " "
<< cam.pos[1] << " "
<< cam.pos[2] << " p\n";
Eigen::Matrix3f m;
m =pose.rotation();
//All axies use right hand rule. x=red axis, y=green axis, z=blue axis z direction is point into the screen. z \ \ \ -----------> x | | | | | | y
cout << pose(0,0) << " " << pose(0,1) << " " << pose(0,2) << " " << pose(0,3) << " x0\n";
cout << pose(1,0) << " " << pose(1,1) << " " << pose(1,2) << " " << pose(1,3) << " x1\n";
cout << pose(2,0) << " " << pose(2,1) << " " << pose(2,2) << " " << pose(2,3)<< "x2\n";
double yaw,pitch, roll;
wRo_to_euler(m,yaw,pitch,roll);
printf("RPY: %f %f %f\n", roll*180/M_PI,pitch*180/M_PI,yaw*180/M_PI);
// matrix->GetElement(1,0);
cout << m(0,0) << " " << m(0,1) << " " << m(0,2) << " " << " x0\n";
cout << m(1,0) << " " << m(1,1) << " " << m(1,2) << " " << " x1\n";
cout << m(2,0) << " " << m(2,1) << " " << m(2,2) << " "<< "x2\n\n";
Eigen::Quaternionf rf;
rf = Eigen::Quaternionf(m);
Eigen::Quaterniond r(rf.w(),rf.x(),rf.y(),rf.z());
Eigen::Isometry3d init_pose;
init_pose.setIdentity();
init_pose.translation() << cam.pos[0], cam.pos[1], cam.pos[2];
//Eigen::Quaterniond m = euler_to_quat(-1.54, 0, 0);
init_pose.rotate(r);
//
std::stringstream ss;
print_Isometry3d(init_pose,ss);
std::cout << "init_pose: " << ss.str() << "\n";
viewer->addCoordinateSystem (1.0,pose);
double tic = getTime();
std::stringstream ss2;
ss2.precision(20);
ss2 << "simulated_pcl_" << tic << ".pcd";
capture(init_pose,ss2.str());
cout << (getTime() -tic) << " sec\n";
// these three variables determine the position and orientation of
// the camera.
// double lookat[3]; - focal location
// double eye[3]; - my location
// double up[3]; - updirection
// std::cout << view[0] << "," << view[1] << "," << view[2]
// cameras.back ().view[2] = renderer->GetActiveCamera ()->GetOrientationWXYZ()[2];
//ViewTransform->GetOrientationWXYZ();
// Superclass::OnKeyUp ();
//.........这里部分代码省略.........
开发者ID:hitsjt,项目名称:StanfordPCL,代码行数:101,代码来源:sim_viewer.cpp
示例5: Draw
void Terrain::Draw(int type, const Camera& camera, const Light& light){
//Get new position of the cube and update the model view matrix
Eigen::Affine3f wMo;//object to world matrix
Eigen::Affine3f cMw;
Eigen::Affine3f proj;
glUseProgram(m_shader);
#ifdef __APPLE__
glBindVertexArrayAPPLE(m_vertexArrayObject);
#else
glBindVertexArray(m_vertexArrayObject);
#endif
glBindBuffer(GL_ARRAY_BUFFER, m_vertexBufferObject);//use as current buffer
GLint world2camera = glGetUniformLocation(m_shader, "cMw");
GLint projection = glGetUniformLocation(m_shader, "proj");
GLint kAmbient = glGetUniformLocation(m_shader,"kAmbient");
GLint kDiffuse = glGetUniformLocation(m_shader,"kDiffuse");
GLint kSpecular = glGetUniformLocation(m_shader,"kSpecular");
GLint shininess = glGetUniformLocation(m_shader,"shininess");
GLint camera_position = glGetUniformLocation(m_shader, "cameraPosition");
GLint light_position = glGetUniformLocation(m_shader, "lightPosition");
//generate the Angel::Angel::Angel::matrixes
proj = Util::Perspective( camera.m_fovy, camera.m_aspect, camera.m_znear, camera.m_zfar );
cMw = camera.m_cMw;//LookAt(camera.position,camera.lookat, camera.up );
Eigen::Vector4f v4color(0.55,0.25,0.08,1.0);
Eigen::Vector4f Ambient;
Ambient = 0.3*v4color;
Eigen::Vector4f Diffuse;
Diffuse = 0.5*v4color;
Eigen::Vector4f Specular(0.3,0.3,0.3,1.0);
glUniformMatrix4fv( world2camera, 1, GL_FALSE, cMw.data() );
glUniformMatrix4fv( projection, 1, GL_FALSE, proj.data() );
glUniform4fv(kAmbient, 1, Ambient.data());
glUniform4fv(kDiffuse, 1, Diffuse.data());
glUniform4fv(kSpecular, 1, Specular.data());
glUniform4fv(camera_position, 1, camera.m_position.data());
glUniform4fv(light_position, 1, light.m_position.data());
glUniform1f(shininess, 10);
switch (type) {
case DRAW_MESH:
glUniform1i(glGetUniformLocation(m_shader, "renderType"), 1);
glDrawArrays(GL_LINES, 0, m_NTrianglePoints);
break;
case DRAW_PHONG:
glUniform1i(glGetUniformLocation(m_shader, "renderType"), 2);
glDrawArrays(GL_TRIANGLES, 0, m_NTrianglePoints);
break;
}
//draw the obstacles
for(int i = 0; i < m_obstacles.size(); i++)
{
m_obstacles[i]->Draw(type,camera, light);
}
for(int i = 0; i < m_foods.size(); i++)
{
m_foods[i]->Draw(type,camera, light);
}
//draw the obstacles
for(int i = 0; i < m_surface_objects.size(); i++)
{
m_surface_objects[i]->Draw(type,camera, light);
}
}
开发者ID:masakinakada,项目名称:Snake2,代码行数:76,代码来源:Terrain.cpp
示例6: optimize
void keypoint_map::optimize() {
g2o::SparseOptimizer optimizer;
optimizer.setVerbose(true);
g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
linearSolver = new g2o::LinearSolverCholmod<
g2o::BlockSolver_6_3::PoseMatrixType>();
g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
g2o::OptimizationAlgorithmLevenberg* solver =
new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);
double focal_length = intrinsics[0];
Eigen::Vector2d principal_point(intrinsics[2], intrinsics[3]);
g2o::CameraParameters * cam_params = new g2o::CameraParameters(focal_length,
principal_point, 0.);
cam_params->setId(0);
if (!optimizer.addParameter(cam_params)) {
assert(false);
}
std::cerr << camera_positions.size() << " " << keypoints3d.size() << " "
<< observations.size() << std::endl;
int vertex_id = 0, point_id = 0;
for (size_t i = 0; i < camera_positions.size(); i++) {
Eigen::Affine3f cam_world = camera_positions[i].inverse();
Eigen::Vector3d trans(cam_world.translation().cast<double>());
Eigen::Quaterniond q(cam_world.rotation().cast<double>());
g2o::SE3Quat pose(q, trans);
g2o::VertexSE3Expmap * v_se3 = new g2o::VertexSE3Expmap();
v_se3->setId(vertex_id);
if (i < 1) {
v_se3->setFixed(true);
}
v_se3->setEstimate(pose);
optimizer.addVertex(v_se3);
vertex_id++;
}
for (size_t i = 0; i < keypoints3d.size(); i++) {
g2o::VertexSBAPointXYZ * v_p = new g2o::VertexSBAPointXYZ();
v_p->setId(vertex_id + point_id);
v_p->setMarginalized(true);
v_p->setEstimate(keypoints3d[i].getVector3fMap().cast<double>());
optimizer.addVertex(v_p);
point_id++;
}
for (size_t i = 0; i < observations.size(); i++) {
g2o::EdgeProjectXYZ2UV * e = new g2o::EdgeProjectXYZ2UV();
e->setVertex(0,
dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertices().find(
vertex_id + observations[i].point_id)->second));
e->setVertex(1,
dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertices().find(
observations[i].cam_id)->second));
e->setMeasurement(observations[i].coord.cast<double>());
e->information() = Eigen::Matrix2d::Identity();
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
e->setParameterId(0, 0);
optimizer.addEdge(e);
}
//optimizer.save("debug.txt");
optimizer.initializeOptimization();
optimizer.setVerbose(true);
std::cout << std::endl;
std::cout << "Performing full BA:" << std::endl;
optimizer.optimize(1);
std::cout << std::endl;
for (int i = 0; i < vertex_id; i++) {
g2o::HyperGraph::VertexIDMap::iterator v_it = optimizer.vertices().find(
i);
if (v_it == optimizer.vertices().end()) {
std::cerr << "Vertex " << i << " not in graph!" << std::endl;
exit(-1);
}
g2o::VertexSE3Expmap * v_c =
dynamic_cast<g2o::VertexSE3Expmap *>(v_it->second);
if (v_c == 0) {
std::cerr << "Vertex " << i << "is not a VertexSE3Expmap!"
<< std::endl;
exit(-1);
}
//.........这里部分代码省略.........
开发者ID:DevasenaInupakutika,项目名称:rapyuta-mapping,代码行数:101,代码来源:keypoint_map.cpp
示例7: mat
void jsk_pcl_ros::DepthImageCreator::publish_points(const sensor_msgs::CameraInfoConstPtr& info,
const sensor_msgs::PointCloud2ConstPtr& pcloud2) {
JSK_ROS_DEBUG("DepthImageCreator::publish_points");
if (!pcloud2) return;
bool proc_cloud = true, proc_image = true, proc_disp = true;
if ( pub_cloud_.getNumSubscribers()==0 ) {
proc_cloud = false;
}
if ( pub_image_.getNumSubscribers()==0 ) {
proc_image = false;
}
if ( pub_disp_image_.getNumSubscribers()==0 ) {
proc_disp = false;
}
if( !proc_cloud && !proc_image && !proc_disp) return;
int width = info->width;
int height = info->height;
float fx = info->P[0];
float cx = info->P[2];
float tx = info->P[3];
float fy = info->P[5];
float cy = info->P[6];
Eigen::Affine3f sensorPose;
{
tf::StampedTransform transform;
if(use_fixed_transform) {
transform = fixed_transform;
} else {
try {
tf_listener_->waitForTransform(pcloud2->header.frame_id,
info->header.frame_id,
info->header.stamp,
ros::Duration(0.001));
tf_listener_->lookupTransform(pcloud2->header.frame_id,
info->header.frame_id,
info->header.stamp, transform);
}
catch ( std::runtime_error e ) {
JSK_ROS_ERROR("%s",e.what());
return;
}
}
tf::Vector3 p = transform.getOrigin();
tf::Quaternion q = transform.getRotation();
sensorPose = (Eigen::Affine3f)Eigen::Translation3f(p.getX(), p.getY(), p.getZ());
Eigen::Quaternion<float> rot(q.getW(), q.getX(), q.getY(), q.getZ());
sensorPose = sensorPose * rot;
if (tx != 0.0) {
Eigen::Affine3f trans = (Eigen::Affine3f)Eigen::Translation3f(-tx/fx , 0, 0);
sensorPose = sensorPose * trans;
}
#if 0 // debug print
JSK_ROS_INFO("%f %f %f %f %f %f %f %f %f, %f %f %f",
sensorPose(0,0), sensorPose(0,1), sensorPose(0,2),
sensorPose(1,0), sensorPose(1,1), sensorPose(1,2),
sensorPose(2,0), sensorPose(2,1), sensorPose(2,2),
sensorPose(0,3), sensorPose(1,3), sensorPose(2,3));
#endif
}
PointCloud pointCloud;
pcl::RangeImagePlanar rangeImageP;
{
// code here is dirty, some bag is in RangeImagePlanar
PointCloud tpc;
pcl::fromROSMsg(*pcloud2, tpc);
Eigen::Affine3f inv;
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 5 )
inv = sensorPose.inverse();
pcl::transformPointCloud< Point > (tpc, pointCloud, inv);
#else
pcl::getInverse(sensorPose, inv);
pcl::getTransformedPointCloud<PointCloud> (tpc, inv, pointCloud);
#endif
Eigen::Affine3f dummytrans;
dummytrans.setIdentity();
rangeImageP.createFromPointCloudWithFixedSize (pointCloud,
width/scale_depth, height/scale_depth,
cx/scale_depth, cy/scale_depth,
fx/scale_depth, fy/scale_depth,
dummytrans); //sensorPose);
}
cv::Mat mat(rangeImageP.height, rangeImageP.width, CV_32FC1);
float *tmpf = (float *)mat.ptr();
for(unsigned int i = 0; i < rangeImageP.height*rangeImageP.width; i++) {
tmpf[i] = rangeImageP.points[i].z;
}
if(scale_depth != 1.0) {
cv::Mat tmpmat(info->height, info->width, CV_32FC1);
cv::resize(mat, tmpmat, cv::Size(info->width, info->height)); // LINEAR
//cv::resize(mat, tmpmat, cv::Size(info->width, info->height), 0.0, 0.0, cv::INTER_NEAREST);
mat = tmpmat;
}
//.........这里部分代码省略.........
开发者ID:Horisu,项目名称:jsk_recognition,代码行数:101,代码来源:depth_image_creator_nodelet.cpp
示例8: cloud_cb
void cloud_cb(
const typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud) {
iter++;
if(iter != skip) return;
iter = 0;
pcl::PointCloud<pcl::PointXYZRGB> cloud_transformed,
cloud_aligned, cloud_filtered;
Eigen::Affine3f view_transform;
view_transform.matrix() << 0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 1;
Eigen::AngleAxis<float> rot(tilt * M_PI / 180,
Eigen::Vector3f(0, 1, 0));
view_transform.prerotate(rot);
pcl::transformPointCloud(*cloud, cloud_transformed, view_transform);
pcl::ModelCoefficients::Ptr coefficients(
new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold(0.05);
seg.setProbability(0.99);
seg.setInputCloud(cloud_transformed.makeShared());
seg.segment(*inliers, *coefficients);
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
extract.setInputCloud(cloud_transformed.makeShared());
extract.setIndices(inliers);
extract.setNegative(true);
extract.filter(cloud_transformed);
std::cout << "Z vector: " << coefficients->values[0] << " "
<< coefficients->values[1] << " " << coefficients->values[2]
<< " " << coefficients->values[3] << std::endl;
Eigen::Vector3f z_current(coefficients->values[0],
coefficients->values[1], coefficients->values[2]);
Eigen::Vector3f y(0, 1, 0);
Eigen::Affine3f rotation;
rotation = pcl::getTransFromUnitVectorsZY(z_current, y);
rotation.translate(Eigen::Vector3f(0, 0, coefficients->values[3]));
pcl::transformPointCloud(cloud_transformed, cloud_aligned, rotation);
Eigen::Affine3f res = (rotation * view_transform);
cloud_aligned.sensor_origin_ = res * Eigen::Vector4f(0, 0, 0, 1);
cloud_aligned.sensor_orientation_ = res.rotate(
Eigen::AngleAxisf(M_PI, Eigen::Vector3f(0, 0, 1))).rotation();
seg.setInputCloud(cloud_aligned.makeShared());
seg.segment(*inliers, *coefficients);
std::cout << "Z vector: " << coefficients->values[0] << " "
<< coefficients->values[1] << " " << coefficients->values[2]
<< " " << coefficients->values[3] << std::endl;
pub.publish(cloud_aligned);
}
开发者ID:vsu91,项目名称:mapping,代码行数:75,代码来源:convert_node.cpp
示例9: execute
void IterativeClosestPoint::execute() {
float error = std::numeric_limits<float>::max(), previousError;
uint iterations = 0;
// Get access to the two point sets
PointSetAccess::pointer accessFixedSet = ((PointSet::pointer)getStaticInputData<PointSet>(0))->getAccess(ACCESS_READ);
PointSetAccess::pointer accessMovingSet = ((PointSet::pointer)getStaticInputData<PointSet>(1))->getAccess(ACCESS_READ);
// Get transformations of point sets
AffineTransformation::pointer fixedPointTransform2 = SceneGraph::getAffineTransformationFromData(getStaticInputData<PointSet>(0));
Eigen::Affine3f fixedPointTransform;
fixedPointTransform.matrix() = fixedPointTransform2->matrix();
AffineTransformation::pointer initialMovingTransform2 = SceneGraph::getAffineTransformationFromData(getStaticInputData<PointSet>(1));
Eigen::Affine3f initialMovingTransform;
initialMovingTransform.matrix() = initialMovingTransform2->matrix();
// These matrices are Nx3
MatrixXf fixedPoints = accessFixedSet->getPointSetAsMatrix();
MatrixXf movingPoints = accessMovingSet->getPointSetAsMatrix();
Eigen::Affine3f currentTransformation = Eigen::Affine3f::Identity();
// Want to choose the smallest one as moving
bool invertTransform = false;
if(false && fixedPoints.cols() < movingPoints.cols()) {
reportInfo() << "switching fixed and moving" << Reporter::end;
// Switch fixed and moving
MatrixXf temp = fixedPoints;
fixedPoints = movingPoints;
movingPoints = temp;
invertTransform = true;
// Apply initial transformations
//currentTransformation = fixedPointTransform.getTransform();
movingPoints = fixedPointTransform*movingPoints.colwise().homogeneous();
fixedPoints = initialMovingTransform*fixedPoints.colwise().homogeneous();
} else {
// Apply initial transformations
//currentTransformation = initialMovingTransform.getTransform();
movingPoints = initialMovingTransform*movingPoints.colwise().homogeneous();
fixedPoints = fixedPointTransform*fixedPoints.colwise().homogeneous();
}
do {
previousError = error;
MatrixXf movedPoints = currentTransformation*(movingPoints.colwise().homogeneous());
// Match closest points using current transformation
MatrixXf rearrangedFixedPoints = rearrangeMatrixToClosestPoints(
fixedPoints, movedPoints);
// Get centroids
Vector3f centroidFixed = getCentroid(rearrangedFixedPoints);
//reportInfo() << "Centroid fixed: " << Reporter::end;
//reportInfo() << centroidFixed << Reporter::end;
Vector3f centroidMoving = getCentroid(movedPoints);
//reportInfo() << "Centroid moving: " << Reporter::end;
//reportInfo() << centroidMoving << Reporter::end;
Eigen::Transform<float, 3, Eigen::Affine> updateTransform = Eigen::Transform<float, 3, Eigen::Affine>::Identity();
if(mTransformationType == IterativeClosestPoint::RIGID) {
// Create correlation matrix H of the deviations from centroid
MatrixXf H = (movedPoints.colwise() - centroidMoving)*
(rearrangedFixedPoints.colwise() - centroidFixed).transpose();
// Do SVD on H
Eigen::JacobiSVD<Eigen::MatrixXf> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
// Estimate rotation as R=V*U.transpose()
MatrixXf temp = svd.matrixV()*svd.matrixU().transpose();
Matrix3f d = Matrix3f::Identity();
d(2,2) = sign(temp.determinant());
Matrix3f R = svd.matrixV()*d*svd.matrixU().transpose();
// Estimate translation
Vector3f T = centroidFixed - R*centroidMoving;
updateTransform.linear() = R;
updateTransform.translation() = T;
} else {
// Only translation
Vector3f T = centroidFixed - centroidMoving;
updateTransform.translation() = T;
}
// Update current transformation
currentTransformation = updateTransform*currentTransformation;
// Calculate RMS error
MatrixXf distance = rearrangedFixedPoints - currentTransformation*(movingPoints.colwise().homogeneous());
error = 0;
for(uint i = 0; i < distance.cols(); i++) {
error += pow(distance.col(i).norm(),2);
}
error = sqrt(error / distance.cols());
iterations++;
reportInfo() << "Error: " << error << Reporter::end;
// To continue, change in error has to be above min error change and nr of iterations less than max iterations
} while(previousError-error > mMinErrorChange && iterations < mMaxIterations);
//.........这里部分代码省略.........
开发者ID:gitter-badger,项目名称:FAST,代码行数:101,代码来源:IterativeClosestPoint.cpp
示例10: simulate_callback
void simulate_callback (const pcl::visualization::KeyboardEvent &event,
void* viewer_void)
{
pcl::visualization::PCLVisualizer::Ptr viewer = *static_cast<pcl::visualization::PCLVisualizer::Ptr *> (viewer_void);
// I choose v for virtual as s for simulate is takwen
if (event.getKeySym () == "v" && event.keyDown ())
{
std::cout << "v was pressed => simulate cloud" << std::endl;
std::vector<pcl::visualization::Camera> cams;
viewer->getCameras(cams);
if (cams.size() !=1){
std::cout << "n cams not 1 exiting\n"; // for now in case ...
return;
}
// cout << "n cams: " << cams.size() << "\n";
pcl::visualization::Camera cam = cams[0];
Eigen::Affine3f pose;
pose = viewer->getViewerPose() ;
std::cout << cam.pos[0] << " "
<< cam.pos[1] << " "
<< cam.pos[2] << " p\n";
Eigen::Matrix3f m;
m =pose.rotation();
//All axies use right hand rule. x=red axis, y=green axis, z=blue axis z direction is point into the screen. z \ \ \ -----------> x | | | | | | y
cout << pose(0,0) << " " << pose(0,1) << " " << pose(0,2) << " " << pose(0,3) << " x0\n";
cout << pose(1,0) << " " << pose(1,1) << " " << pose(1,2) << " " << pose(1,3) << " x1\n";
cout << pose(2,0) << " " << pose(2,1) << " " << pose(2,2) << " " << pose(2,3)<< "x2\n";
double yaw,pitch, roll;
wRo_to_euler(m,yaw,pitch,roll);
printf("RPY: %f %f %f\n", roll*180/M_PI,pitch*180/M_PI,yaw*180/M_PI);
// matrix->GetElement(1,0);
cout << m(0,0) << " " << m(0,1) << " " << m(0,2) << " " << " x0\n";
cout << m(1,0) << " " << m(1,1) << " " << m(1,2) << " " << " x1\n";
cout << m(2,0) << " " << m(2,1) << " " << m(2,2) << " "<< "x2\n\n";
Eigen::Quaternionf rf;
rf = Eigen::Quaternionf(m);
Eigen::Quaterniond r(rf.w(),rf.x(),rf.y(),rf.z());
Eigen::Isometry3d init_pose;
init_pose.setIdentity();
init_pose.translation() << cam.pos[0], cam.pos[1], cam.pos[2];
//Eigen::Quaterniond m = euler_to_quat(-1.54, 0, 0);
init_pose.rotate(r);
//
std::stringstream ss;
print_Isometry3d(init_pose,ss);
std::cout << "init_pose: " << ss.str() << "\n";
viewer->addCoordinateSystem (1.0,pose,"reference");
double tic = getTime();
std::stringstream ss2;
ss2.precision(20);
ss2 << "simulated_pcl_" << tic << ".pcd";
capture(init_pose);
cout << (getTime() -tic) << " sec\n";
}
}
开发者ID:hobu,项目名称:pcl,代码行数:81,代码来源:sim_viewer.cpp
示例11: setShapePosition
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 = frame_id_;
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(unsigned 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);
/* the name of the marker is arrows_shape_.id, we need to erase the "arrows_" part */
// int test ;
// string strName(feedback->marker_name);
// strName.erase(strName.begin(),strName.begin()+7);
// stringstream nameTest(strName);
// std::cout << "nameTest : " << nameTest.str() << "\n" ;
// nameTest >> test ;
// std::cout << "test : " << test << "\n" ;
// name >> shape_id ;
cob_3d_mapping::Polygon p;
// for(size_t i=0;i<sha.shapes.size();++i)
// {
// if (sha.shapes[i].id == shape_id)
// {
// index = i;
// }
// }
std::cout << "index is : " << index << "\n" ;
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 ;
//.........这里部分代码省略.........
开发者ID:srs-user-tests,项目名称:cob_environment_perception,代码行数:101,代码来源:shape_visualization.cpp
示例12: cloudCallback
void PointCloudLocalization::cloudCallback(
const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
{
vital_checker_->poke();
boost::mutex::scoped_lock lock(mutex_);
//JSK_NODELET_INFO("cloudCallback");
latest_cloud_ = cloud_msg;
if (localize_requested_){
JSK_NODELET_INFO("localization is requested");
try {
pcl::PointCloud<pcl::PointNormal>::Ptr
local_cloud (new pcl::PointCloud<pcl::PointNormal>);
pcl::fromROSMsg(*latest_cloud_, *local_cloud);
JSK_NODELET_INFO("waiting for tf transformation from %s tp %s",
latest_cloud_->header.frame_id.c_str(),
global_frame_.c_str());
if (tf_listener_->waitForTransform(
latest_cloud_->header.frame_id,
global_frame_,
latest_cloud_->header.stamp,
ros::Duration(1.0))) {
pcl::PointCloud<pcl::PointNormal>::Ptr
input_cloud (new pcl::PointCloud<pcl::PointNormal>);
if (use_normal_) {
pcl_ros::transformPointCloudWithNormals(global_frame_,
*local_cloud,
*input_cloud,
*tf_listener_);
}
else {
pcl_ros::transformPointCloud(global_frame_,
*local_cloud,
*input_cloud,
*tf_listener_);
}
pcl::PointCloud<pcl::PointNormal>::Ptr
input_downsampled_cloud (new pcl::PointCloud<pcl::PointNormal>);
applyDownsampling(input_cloud, *input_downsampled_cloud);
if (isFirstTime()) {
all_cloud_ = input_downsampled_cloud;
first_time_ = false;
}
else {
// run ICP
ros::ServiceClient client
= pnh_->serviceClient<jsk_pcl_ros::ICPAlign>("icp_align");
jsk_pcl_ros::ICPAlign icp_srv;
if (clip_unseen_pointcloud_) {
// Before running ICP, remove pointcloud where we cannot see
// First, transform reference pointcloud, that is all_cloud_, into
// sensor frame.
// And after that, remove points which are x < 0.
tf::StampedTransform global_sensor_tf_transform
= lookupTransformWithDuration(
tf_listener_,
global_frame_,
sensor_frame_,
cloud_msg->header.stamp,
ros::Duration(1.0));
Eigen::Affine3f global_sensor_transform;
tf::transformTFToEigen(global_sensor_tf_transform,
global_sensor_transform);
pcl::PointCloud<pcl::PointNormal>::Ptr sensor_cloud
(new pcl::PointCloud<pcl::PointNormal>);
pcl::transformPointCloudWithNormals(
*all_cloud_,
*sensor_cloud,
global_sensor_transform.inverse());
// Remove negative-x points
pcl::PassThrough<pcl::PointNormal> pass;
pass.setInputCloud(sensor_cloud);
pass.setFilterFieldName("x");
pass.setFilterLimits(0.0, 100.0);
pcl::PointCloud<pcl::PointNormal>::Ptr filtered_cloud
(new pcl::PointCloud<pcl::PointNormal>);
pass.filter(*filtered_cloud);
JSK_NODELET_INFO("clipping: %lu -> %lu", sensor_cloud->points.size(), filtered_cloud->points.size());
// Convert the pointcloud to global frame again
pcl::PointCloud<pcl::PointNormal>::Ptr global_filtered_cloud
(new pcl::PointCloud<pcl::PointNormal>);
pcl::transformPointCloudWithNormals(
*filtered_cloud,
*global_filtered_cloud,
global_sensor_transform);
pcl::toROSMsg(*global_filtered_cloud,
icp_srv.request.target_cloud);
}
else {
pcl::toROSMsg(*all_cloud_,
icp_srv.request.target_cloud);
}
pcl::toROSMsg(*input_downsampled_cloud,
icp_srv.request.reference_cloud);
if (client.call(icp_srv)) {
Eigen::Affine3f transform;
tf::poseMsgToEigen(icp_srv.response.result.pose, transform);
Eigen::Vector3f transform_pos(transform.translation());
float roll, pitch, yaw;
//.........这里部分代码省略.........
开发者ID:YuOhara,项目名称:jsk_recognition,代码行数:101,代码来源:pointcloud_localization_nodelet.cpp
示例13: if
void
pcl::CropBox<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
{
// Resize output cloud to sample size
output.data.resize (input_->data.size ());
removed_indices_->resize (input_->data.size ());
// Copy the common fields
output.fields = input_->fields;
output.is_bigendian = input_->is_bigendian;
output.row_step = input_->row_step;
output.point_step = input_->point_step;
output.height = 1;
int indices_count = 0;
int removed_indices_count = 0;
Eigen::Affine3f transform = Eigen::Affine3f::Identity();
Eigen::Affine3f inverse_transform = Eigen::Affine3f::Identity();
if (rotation_ != Eigen::Vector3f::Zero ())
{
pcl::getTransformation (0, 0, 0,
rotation_ (0), rotation_ (1), rotation_ (2),
transform);
inverse_transform = transform.inverse();
}
//PointXYZ local_pt;
Eigen::Vector3f local_pt (Eigen::Vector3f::Zero ());
for (size_t index = 0; index < indices_->size (); ++index)
{
// Get local point
int point_offset = ((*indices_)[index] * input_->point_step);
int offset = point_offset + input_->fields[x_idx_].offset;
memcpy (&local_pt, &input_->data[offset], sizeof (float)*3);
// Check if the point is invalid
if (!pcl_isfinite (local_pt.x ()) ||
!pcl_isfinite (local_pt.y ()) ||
!pcl_isfinite (local_pt.z ()))
continue;
// Transform point to world space
if (!(transform_.matrix().isIdentity()))
local_pt = transform_ * local_pt;
if (translation_ != Eigen::Vector3f::Zero ())
{
local_pt.x () = local_pt.x () - translation_ (0);
local_pt.y () = local_pt.y () - translation_ (1);
local_pt.z () = local_pt.z () - translation_ (2);
}
// Transform point to local space of crop box
if (!(inverse_transform.matrix ().isIdentity ()))
local_pt = inverse_transform * local_pt;
// If outside the cropbox
if ( (local_pt.x () < min_pt_[0] || local_pt.y () < min_pt_[1] || local_pt.z () < min_pt_[2]) ||
(local_pt.x () > max_pt_[0] || local_pt.y () > max_pt_[1] || local_pt.z () > max_pt_[2]))
{
if (negative_)
{
memcpy (&output.data[indices_count++ * output.point_step],
&input_->data[index * output.point_step], output.point_step);
}
else if (extract_removed_indices_)
{
(*removed_indices_)[removed_indices_count++] = static_cast<int> (index);
}
}
|
请发表评论