- 除执行三个任务，depth_sector_area要同时识别噪声和盲区。
一、depth_sector_area
根据深度数据，将预设扇区内的那部分生成一个LaserScan，放入变量scan_msg。在存入的同时，还会根据不同场景，执行两个操作。
- mutable_pixels != nullptr && points == nullptr。叠加到数据区。这一般用在可视化深度数据。
- mutable_pixels == nullptr && points != nullptr。导航时，要把扇区内数据放入points，以便和激光雷达融合。
mutable_pixels、points是互斥的，不可能同时是nullptr，或同时不是nullptr。
- @intrinsics。相机内参。只接收深度时，用相机深度内参。接收深度+RGB时，用RGB内参。
- @scan_msg[OUT]。用于存储相机的深度点云。
- @width、height。深度帧宽度、高度。
- @depth_data。深度值。
- @depth_scale。深度值比使系数。一般是1.0。
- @mutable_pixels[OUT]。不为nullptr时，用于指示将点云叠加到该ARGB图面上。
- @points[OUT]。不为nullptr时，用于存储深度点云转换后的tfpoint3_buffer格数据。
- @dcpitch。此时相机俯仰角。float_nposm时，表示没有不必考虑俯仰角旋转。不管是不是float_nposm，都会转到world坐标系。
tdepth_sector_result depth_sector_area(const tdcintrinsics_C& intrinsics, sensor_msgs::LaserScan& scan_msg,
int width, int height, const uint16_t* depth_data, double depth_scale,
uint8_t* mutable_pixels, tfpoint3_buffer* points, double dcpitch);既然深度要被覆盖掉mutable_pixels，或放入points，为什么还要额外生成个scan_msg？——在一些场景，希望要用生成的scan_msg，快速发出一个以深度数据为内容LaserScan话题，话题名：camera_scan。
除上面说的3个任务，depth_sector_area要执行两种识别。一是识别是否噪声，如果有噪声，后绪要扔掉。二是识别盲区情况，方便后绪根据盲区做进一步判断。
1.1 可视化深度图
mutable_pixels != nullptr && points == nullptr，会视频可化深度图。此时mutable_pixels是一块归属ARGB格式图面的内存块。此时depth_sector_area任务是根据像素上障碍物情况，给该像素填上三种背景色。示例见下文图1。
- 黄色：有效障碍物。其它颜色都表示不是有效障碍物。
- 青色：高度是满足条件了，距离不满足[scan_msg.range_min, scan_msg.range_max]。
- 灰色：高度是满足条件了，角度不满足[scan_msg.angle_min, scan_msg.angle_max]。
- (不填)：高度不满足要求。
在launcher的“深度相机”界面可生成两种可视化深度图图像，分别是dbg_depth.png、dbg_depth_all_rows.png。为什么要存在两种呢？
假设深度帧分辨率是1280x800，每个点要18个像素显示数值，采用4通道ARGB，即完整显示的话要1280 x 18 x 800 x 18 x 4个字节。分配这么大字节数会失败。为减少须要字节数，于是减少垂直、水平各取一半。显示出来的只是偶数索引单元，即0、2、4、6、8、10、12、14...。dbg_depth.png存的就是这种垂直、水平各取一半时的文件。
但在一场景，像要直观看到噪声情况，这时更希望显示全部行，列的话可以四取一，dbg_depth_all_rows.png对应的就是此种文件。
1.2 识别噪声
在“测试素材”“dcamera-noised/example”可下载图1完整图像。此时相机对着一个块平坦地面，理论上是不该有障碍物的。
图1是一个发生噪声时深度图。注意黄色数值所在行（一次红、绿、蓝3坐标行表示一行像素）、以及它的下一行。3坐标中蓝色数字是从相机读到的深度，这在两行附近，由上到下，深度一般在-502、-501、-503，而且整行深度都有规律。但在那一行，深度突然变了，很突兀，而且是整个一行都这样。但过了一两行，深度又恢复到-501、-500、这样有规律了，就是继续之前趋势。所以这一行数据感觉就不是正常数据的一部分，噪音。
这噪音会给导航带来大麻烦，像把本该没有障碍的报出有障碍。以下是检测噪声办法。
- 取中间20个，如果有10个发生了突变(前后相差2cm)，那认为此行可能是噪声。
- 如果连续两行可能是噪声，那认为此帧存在噪声。
奥比中光说噪声可能是两种原因。一是电流不够，有时会超过500ma，USB2.0就不够了，至少USB3.0。2）是USB带宽不够，建议用OB_FORMAT_Y14。为保证条件，会用USB3.0。对第二点，于少在windows，启动时用OB_FORMAT_Y14，但过程中收深度数据时，格式又是变成OB_FORMAT_Y16，而此过过也是会噪声。到现在没解决这问题。
出噪音概率不低。宁愿把正确的点云识别为错误，不能把错误的漏掉，会把好多没噪音的也扔掉。但是，也会出现一些噪声检测不出。
1.3 识别盲区
这里所说的盲区就是障碍物因为太靠近相机，给相机造成盲区。
在“测试素材”“dcamera-no_depth/example”可下载图2完整图像。此时相机右则贴近根椅脚，椅脚给相机造成盲区。
一旦距离过近，使得深度相机进入盲区，包括只是部分近入，那也要避免的。解决办法是机器人后退，先退出盲区。退的办法是，告知前面是全障碍物。以1280x800深度帧为例说下识别盲区方法。
- 分左、右两半，left_no_depth、right_no_depth分别表示它们是否有盲区。为什么要分左、右两个？——障碍物可能出现在只偏左、只偏右，要是一半有就认要机器人后退，那影响过大了。
- 对每个半区，只管上部分，即区域的尺寸是640x400像素。为什么每个半区，只管上部分？——一旦造成盲区，对深度图来说，那是从上到下，即y索引从0变大。造成盲区时，下部分有可能是有深度的。只管上管区，避免下半区给造成干扰。
- 640x400区域内，如果有40%的列，它们没有深度的行数超过65%的总行数，那认为该半区没有深度。——40%的列，是让前面有部分挡住就后退。镜子等物体也可能造成没深度，具体到某一列，设个65%的没深度阈值。
1.4 depth_sector_area取的角度范围需大于60(2*moveit_sector_hfangle_deg)
depth_sector_area角度范围是来自深度相机原点(dcamera)，moveit_sector_hfangle_deg则来自激光雷达原点(laser)，对空间同一个点，它们会出来各自角度。图3中A点，laser认为它在60度内，dcamera则认为60度外，但在90度内。图3表示了laser、dcamera的位置关系，因而要保证laser的60，dcamera的角度范围需大于60。
目前dcamera角度范围[scan_msg.angle_min, scan_msg.angle_max]取的是90。
depth_sector_area会把60范围外的点也写到points，这会不会给后面laser处理造成问题？——不会。融合操作(integrate_dcamera_LaserScan)会剔除那些在laser认为60度外的点。
在x方向，相机位在雷达前面，在y方向，相机会雷达也有偏移。因为这两个偏移，使得空间中同一个点，在雷达中满足60度，但要在相机是多少范围。这就须要种方法快速判断相机设的角度范围，是否包括了雷达的60度范围内的所有障碍点
- 测试环境，在机器人正前方放一块长横板，平行着机器人放，并让机器人正前的障碍物全是横板。
- 横板平行机器人，在rviz能看到，这条点云（绿色）是竖直的。白色是补出的点云，正确的话，它几乎是粘贴着绿色，
1.5 min_z
在z轴，超过min_z高度的，认为是障碍物。图4是min_z在空间的示意图。以下是计算公式。
float min_z = -1 * (ros::dcamera_height_from_ground - ros::safe_obstacle_height); 样例：-0.32 = (0.5 - 0.18)
在world坐标系，在相机高度时，world_xyz2.z=0，往下的是负数，越往下绝对值越大。为方便和world_xyz2.z比较，min_z是个负值。如果满足“world_xyz2.z >= min_z”，那认为world_xyz2这个坐标点存在障碍物。
如何设定ros::dcamera_height_from_ground？——让机械臂处在state_navigation，看地面的world_z，取绝对值。
如何设定ros::safe_obstacle_height？——相机测得数据不是很准，需要留出冗余，为此需要比希望的大些。目前取的是0.18m。
1.6 scan_msg.range_min、scan_msg.range_max
在x轴方向，机器人本身部分会向前越过相机原点，像机械臂爪子末端，但不能被认为障碍物。那这个最小world.x要取多少？——scan_msg.range_min。scan_msg.range_min用于指示，以相机原点为中心，要超过多少半径的物体，才认为是障碍物。障碍物会有个范围[scan_msg.range_min, scan_msg.range_max]，目前取的是[0.07, 2.0]。即只有半径>=7cm了，才认为是障碍物。
如何设定range_min？——爪子末端是最大x，要超过这个值。为冗余，至少要多出1cm。目前是0.07m。
这个距离是指从相机原点开始，不是从base_footprint。而且机械臂放在底盘哪位置对它无影响。