无锡网站优化推广方案国内广告投放平台

以前以为rviz是用OpenGL渲染绘图，那么获取图像里像素点对应的真实3D坐标是采用的OpenGL里提供的API实现的，结果一看代码还真不是这样，rviz也就渲染用了OpenGL，其他都是自己实现的，图像界面的实现完全是遵循MVC设计模式自己实现的透视投影和坐标转换等所有相关类。获取点云图像里所选择的点云点的3D坐标相关的代码是这里：

src/rviz/selection/selection_manager.cpp:bool SelectionManager::getPatchDepthImage(Ogre::Viewport* viewport,int x,int y,unsigned width,unsigned height,std::vector<float>& depth_vector)
{unsigned int num_pixels = width * height;depth_vector.reserve(num_pixels);setDepthTextureSize(width, height);M_CollisionObjectToSelectionHandler::iterator handler_it = objects_.begin();M_CollisionObjectToSelectionHandler::iterator handler_end = objects_.end();for (; handler_it != handler_end; ++handler_it){handler_it->second->preRenderPass(0);}if (render(viewport, depth_render_texture_, x, y, x + width, y + height, depth_pixel_box_, "Depth",depth_texture_width_, depth_texture_height_)){uint8_t* data_ptr = (uint8_t*)depth_pixel_box_.data;for (uint32_t pixel = 0; pixel < num_pixels; ++pixel){uint8_t a = data_ptr[4 * pixel];uint8_t b = data_ptr[4 * pixel + 1];uint8_t c = data_ptr[4 * pixel + 2];int int_depth = (c << 16) | (b << 8) | a;float normalized_depth = ((float)int_depth) / (float)0xffffff;depth_vector.push_back(normalized_depth * camera_->getFarClipDistance());}}else{ROS_WARN("Failed to render depth patch\n");return false;}handler_it = objects_.begin();handler_end = objects_.end();for (; handler_it != handler_end; ++handler_it){handler_it->second->postRenderPass(0);}return true;
}bool SelectionManager::get3DPatch(Ogre::Viewport* viewport,int x,int y,unsigned width,unsigned height,bool skip_missing,std::vector<Ogre::Vector3>& result_points)
{boost::recursive_mutex::scoped_lock lock(global_mutex_);ROS_DEBUG("SelectionManager.get3DPatch()");std::vector<float> depth_vector;if (!getPatchDepthImage(viewport, x, y, width, height, depth_vector))return false;unsigned int pixel_counter = 0;Ogre::Matrix4 projection = camera_->getProjectionMatrix();float depth;for (unsigned y_iter = 0; y_iter < height; ++y_iter)for (unsigned x_iter = 0; x_iter < width; ++x_iter){depth = depth_vector[pixel_counter];// Deal with missing or invalid pointsif ((depth > camera_->getFarClipDistance()) || (depth == 0)){++pixel_counter;if (!skip_missing){result_points.push_back(Ogre::Vector3(NAN, NAN, NAN));}continue;}Ogre::Vector3 result_point;// We want to shoot rays through the center of pixels, not the corners,// so add .5 pixels to the x and y coordinate to get to the center// instead of the top left of the pixel.Ogre::Real screenx = float(x_iter + .5) / float(width);Ogre::Real screeny = float(y_iter + .5) / float(height);if (projection[3][3] == 0.0) // If this is a perspective projection{// get world-space ray from camera & mouse coordOgre::Ray vp_ray = camera_->getCameraToViewportRay(screenx, screeny);// transform ray direction back into camera coordsOgre::Vector3 dir_cam = camera_->getDerivedOrientation().Inverse() * vp_ray.getDirection();// normalize, so dir_cam.z == -depthdir_cam = dir_cam / dir_cam.z * depth * -1;// compute 3d point from camera origin and direction*/result_point = camera_->getDerivedPosition() + camera_->getDerivedOrientation() * dir_cam;}else // else this must be an orthographic projection.{// For orthographic projection, getCameraToViewportRay() does// the right thing for us, and the above math does not work.Ogre::Ray ray;camera_->getCameraToViewportRay(screenx, screeny, &ray);result_point = ray.getPoint(depth);}result_points.push_back(result_point);++pixel_counter;}return !result_points.empty();
}bool SelectionManager::get3DPoint(Ogre::Viewport* viewport, int x, int y, Ogre::Vector3& result_point)
{ROS_DEBUG("SelectionManager.get3DPoint()");std::vector<Ogre::Vector3> result_points_temp;bool success = get3DPatch(viewport, x, y, 1, 1, true, result_points_temp);if (result_points_temp.empty()){// return result_point unmodified if get point fails.return false;}result_point = result_points_temp[0];return success;
}

世界3D坐标是用的射线法计算出来。SelectionManager::get3DPoint()被rviz里多个地方调用，凡是UI界面上需要查看点的坐标地方都是调用它。