zqjia
/
Sematic-Cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
Sematic-Cartographer/cartographer-master/cartographer/mapping/internal/2d/ray_to_pixel_mask.cc

708 lines
26 KiB
C++
Raw Normal View History

the first version
2022-06-23 19:58:36 +08:00
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "cartographer/mapping/internal/2d/ray_to_pixel_mask.h"
#include "Eigen/Dense"
int cnew[4000000]={0};
namespace cartographer {
namespace mapping {
namespace {
bool isEqual(const Eigen::Array2i& lhs, const Eigen::Array2i& rhs) {
return ((lhs - rhs).matrix().lpNorm<1>() == 0);
}
} // namespace
// Compute all pixels that contain some part of the line segment connecting
// 'scaled_begin' and 'scaled_end'. 'scaled_begin' and 'scaled_end' are scaled
// by 'subpixel_scale'. 'scaled_begin' and 'scaled_end' are expected to be
// greater than zero. Return values are in pixels and not scaled.
std::vector<Eigen::Array2i> RayToPixelMask(const Eigen::Array2i& scaled_begin,
const Eigen::Array2i& scaled_end,
int subpixel_scale) {
// For simplicity, we order 'scaled_begin' and 'scaled_end' by their x
// coordinate.
if (scaled_begin.x() > scaled_end.x()) {
return RayToPixelMask(scaled_end, scaled_begin, subpixel_scale);
}
CHECK_GE(scaled_begin.x(), 0);
CHECK_GE(scaled_begin.y(), 0);
CHECK_GE(scaled_end.y(), 0);
std::vector<Eigen::Array2i> pixel_mask;
// Special case: We have to draw a vertical line in full pixels, as
// 'scaled_begin' and 'scaled_end' have the same full pixel x coordinate.
if (scaled_begin.x() / subpixel_scale == scaled_end.x() / subpixel_scale) {
Eigen::Array2i current(
scaled_begin.x() / subpixel_scale,
std::min(scaled_begin.y(), scaled_end.y()) / subpixel_scale);
pixel_mask.push_back(current);
const int end_y =
std::max(scaled_begin.y(), scaled_end.y()) / subpixel_scale;
for (; current.y() <= end_y; ++current.y()) {
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
}
return pixel_mask;
}
const int64 dx = scaled_end.x() - scaled_begin.x();
const int64 dy = scaled_end.y() - scaled_begin.y();
const int64 denominator = 2 * subpixel_scale * dx;
// The current full pixel coordinates. We scaled_begin at 'scaled_begin'.
Eigen::Array2i current = scaled_begin / subpixel_scale;
pixel_mask.push_back(current);
// To represent subpixel centers, we use a factor of 2 * 'subpixel_scale' in
// the denominator.
// +-+-+-+ -- 1 = (2 * subpixel_scale) / (2 * subpixel_scale)
// | | | |
// +-+-+-+
// | | | |
// +-+-+-+ -- top edge of first subpixel = 2 / (2 * subpixel_scale)
// | | | | -- center of first subpixel = 1 / (2 * subpixel_scale)
// +-+-+-+ -- 0 = 0 / (2 * subpixel_scale)
// The center of the subpixel part of 'scaled_begin.y()' assuming the
// 'denominator', i.e., sub_y / denominator is in (0, 1).
int64 sub_y = (2 * (scaled_begin.y() % subpixel_scale) + 1) * dx;
// The distance from the from 'scaled_begin' to the right pixel border, to be
// divided by 2 * 'subpixel_scale'.
const int first_pixel =
2 * subpixel_scale - 2 * (scaled_begin.x() % subpixel_scale) - 1;
// The same from the left pixel border to 'scaled_end'.
const int last_pixel = 2 * (scaled_end.x() % subpixel_scale) + 1;
// The full pixel x coordinate of 'scaled_end'.
const int end_x = std::max(scaled_begin.x(), scaled_end.x()) / subpixel_scale;
// Move from 'scaled_begin' to the next pixel border to the right.
sub_y += dy * first_pixel;
if (dy > 0) {
while (true) {
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
while (sub_y > denominator) {
sub_y -= denominator;
++current.y();
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
}
++current.x();
if (sub_y == denominator) {
sub_y -= denominator;
++current.y();
}
if (current.x() == end_x) {
break;
}
// Move from one pixel border to the next.
sub_y += dy * 2 * subpixel_scale;
}
// Move from the pixel border on the right to 'scaled_end'.
sub_y += dy * last_pixel;
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
while (sub_y > denominator) {
sub_y -= denominator;
++current.y();
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
}
CHECK_NE(sub_y, denominator);
CHECK_EQ(current.y(), scaled_end.y() / subpixel_scale);
return pixel_mask;
}
// Same for lines non-ascending in y coordinates.
while (true) {
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
while (sub_y < 0) {
sub_y += denominator;
--current.y();
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
}
++current.x();
if (sub_y == 0) {
sub_y += denominator;
--current.y();
}
if (current.x() == end_x) {
break;
}
sub_y += dy * 2 * subpixel_scale;
}
sub_y += dy * last_pixel;
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
while (sub_y < 0) {
sub_y += denominator;
--current.y();
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
}
CHECK_NE(sub_y, 0);
CHECK_EQ(current.y(), scaled_end.y() / subpixel_scale);
return pixel_mask;
}
Eigen::Affine3d ToEigen(const ::cartographer::transform::Rigid3d& rigid3) {
return Eigen::Translation3d(rigid3.translation()) * rigid3.rotation();
}
/**
* @brief 根据local_pose将miss点转换成全局坐标
*
* @param[in]
* @param[in]
* @param[in]
* @param[in]
* @param[in]
*/
std::vector<Eigen::Array2i> LocalToMap(const Eigen::Array2i& pixel,
const Eigen::Array2i& scaled_begin,
const Eigen::Array2i& scaled_end,
int subpixel_scale,
int flag,
const transform::Rigid3d local_pose_){
int o_x=1000;
int o_y=1000;
int xm;
int ym;
int xm_begin;
int ym_begin;
int xm_end;
int ym_end;
const Eigen::Matrix4d homo =ToEigen(local_pose_).matrix();
if (flag==0){
if (scaled_begin.y()/1000>150){
xm=-(pixel).y()+20*homo(0,3)+(o_x+200);
ym=(pixel).x()-20*homo(1,3)+(o_y-200);
xm_begin=-scaled_begin.y()/1000+20*homo(0,3)+(o_x+200);
ym_begin=scaled_begin.x()/1000-20*homo(1,3)+(o_y-200);
xm_end=-scaled_begin.y()/1000+20*homo(0,3)+(o_x+200);
ym_end=scaled_begin.x()/1000-20*homo(1,3)+(o_y-200);
}else{
xm=-(pixel).y()+20*homo(0,3)+(o_x+100);
ym=(pixel).x()-20*homo(1,3)+(o_y-100);
xm_begin=-scaled_begin.y()/1000+20*homo(0,3)+(o_x+100);
ym_begin=scaled_begin.x()/1000-20*homo(1,3)+(o_y-100);
xm_end=-scaled_end.y()/1000+20*homo(0,3)+(o_x+100);
ym_end=scaled_end.x()/1000-20*homo(1,3)+(o_y-100);
}
}else{
if (scaled_end.y()/1000>150){
xm=-(pixel).y()+20*homo(0,3)+(o_x+200);
ym=(pixel).x()-20*homo(1,3)+(o_y-200);
xm_begin=-scaled_end.y()/1000+20*homo(0,3)+(o_x+200);
ym_begin=scaled_end.x()/1000-20*homo(1,3)+(o_y-200);
xm_end=-scaled_begin.y()/1000+20*homo(0,3)+(o_x+200);
ym_end=scaled_begin.x()/1000-20*homo(1,3)+(o_y-200);
}else{
xm=-(pixel).y()+20*homo(0,3)+(o_x+100);
ym=(pixel).x()-20*homo(1,3)+(o_y-100);
xm_begin=-scaled_end.y()/1000+20*homo(0,3)+(o_x+100);
ym_begin=scaled_end.x()/1000-20*homo(1,3)+(o_y-100);
xm_end=-scaled_begin.y()/1000+20*homo(0,3)+(o_x+100);
ym_end=scaled_begin.x()/1000-20*homo(1,3)+(o_y-100);
// LOG(INFO)<<"wyk--begin is:"<<xm_begin<<" ni "<<ym_begin;
// LOG(INFO)<<"wyk--end is:"<<xm_end<<" end "<<ym_end;
// LOG(INFO)<<"wyk--xiaowuti is:"<<xm<<" "<<ym;
}
}
std::vector<Eigen::Array2i> map_position;
map_position.push_back(Eigen::Array2i(xm_begin, ym_begin));
map_position.push_back(Eigen::Array2i(xm,ym));
map_position.push_back(Eigen::Array2i(xm_end,ym_end));
return map_position;
}
/**
* @brief 非摄像头视域或摄像头视域超过两米范围的小物体不更新，即该范围内的删去miss点
*
*/
std::vector<Eigen::Array2i> ProcessPixelMask(std::vector<Eigen::Array2i> pixel_mask,
const Eigen::Array2i& scaled_begin,
const Eigen::Array2i& scaled_end,
int subpixel_scale,
int flag,
const uint8 intensities,
const transform::Rigid3d local_pose_){
int xm;
int ym;
int xm_begin=LocalToMap(scaled_begin,scaled_begin,scaled_end,subpixel_scale,flag,local_pose_)[0].x();
int ym_begin=LocalToMap(scaled_begin,scaled_begin,scaled_end,subpixel_scale,flag,local_pose_)[0].y();
int xm_end;
int ym_end;
for(std::vector<Eigen::Array2i>::iterator pixel=pixel_mask.begin();pixel!=pixel_mask.end(); ){
xm=LocalToMap(*pixel,scaled_begin,scaled_end,subpixel_scale,flag,local_pose_)[1].x();
ym=LocalToMap(*pixel,scaled_begin,scaled_end,subpixel_scale,flag,local_pose_)[1].y();
if(0<ym*2000+xm && ym*2000+xm<4000000){
//标志是否删去miss 0代表没有删去
int flag_miss=0;
//以小物体为中心2*2搜索
for (int i=-3;i<=3;i++){
for (int j=-3;j<=3;j++){
if (cnew[(ym+j)*2000+xm+i]==115 || cnew[(ym+j)*2000+xm+i]==117 || cnew[(ym+j)*2000+xm+i]==121 || cnew[(ym+j)*2000+xm+i]==200 ||
cnew[(ym+j)*2000+xm+i]==201 || cnew[(ym+j)*2000+xm+i]==205 || cnew[(ym+j)*2000+xm+i]==153 ){
//如果在非摄像头视域
if (intensities == 0) {
pixel=pixel_mask.erase(pixel);
//probability_grid->ApplyLookupTable(*pixel, hit_table);
flag_miss=1;
break;
//在摄像头视域
}else{
//超过两米 小物体不更新
if ((xm-xm_begin)*(xm-xm_begin)+(ym-ym_begin)*(ym-ym_begin)>1600){
pixel=pixel_mask.erase(pixel);
flag_miss=1;
break;
}
}
}
}
if (flag_miss==1)
break;
}
if (flag_miss == 0)
++pixel;
}
else{
LOG(INFO)<<"frame erro:"<<xm<<" "<<ym;
}
}
return pixel_mask;
}
std::vector<Eigen::Array2i> ProcessPixelMaskHit(std::vector<Eigen::Array2i> pixel_mask,
const Eigen::Array2i& scaled_begin,
const Eigen::Array2i& scaled_end,
int subpixel_scale,
int flag,
const uint8 intensities,
const transform::Rigid3d local_pose_,
const std::vector<uint16>& hit_table,
cartographer::mapping::ProbabilityGrid* probability_grid){
int xm;
int ym;
int xm_begin=LocalToMap(scaled_begin,scaled_begin,scaled_end,subpixel_scale,flag,local_pose_)[0].x();
int ym_begin=LocalToMap(scaled_begin,scaled_begin,scaled_end,subpixel_scale,flag,local_pose_)[0].y();
int xm_end;
int ym_end;
for(std::vector<Eigen::Array2i>::iterator pixel=pixel_mask.begin();pixel!=pixel_mask.end(); ){
xm=LocalToMap(*pixel,scaled_begin,scaled_end,subpixel_scale,flag,local_pose_)[1].x();
ym=LocalToMap(*pixel,scaled_begin,scaled_end,subpixel_scale,flag,local_pose_)[1].y();
if(0<ym*2000+xm && ym*2000+xm<4000000){
//标志是否删去miss 0代表没有删去
int flag_miss=0;
//以小物体为中心2*2搜索
for (int i=-1;i<=1;i++){
for (int j=-1;j<=1;j++){
if (cnew[(ym+j)*2000+xm+i]>100 ){
//如果在非摄像头视域
if (intensities == 0) {
//pixel=pixel_mask.erase(pixel);
probability_grid->ApplyLookupTable(*pixel, hit_table);
// for(std::vector<Eigen::Array2i>::iterator pixel_hit=pixel_mask.begin();pixel!=pixel_mask.end(); ){
// probability_grid->ApplyLookupTable(*pixel_hit, hit_table);
// }
++pixel;
flag_miss=1;
break;
}
}
}
if (flag_miss==1)
break;
}
if (flag_miss == 0)
++pixel;
}
else{
LOG(INFO)<<"frame erro:"<<xm<<" "<<ym;
}
}
return pixel_mask;
}
std::vector<Eigen::Array2i> RayToPixelMaskVisualNew(const Eigen::Array2i& scaled_begin,
const Eigen::Array2i& scaled_end,
int subpixel_scale,
const uint8 intensities,
const transform::Rigid3d local_pose_,
int flag,
const std::vector<uint16>& hit_table,
cartographer::mapping::ProbabilityGrid* probability_grid) {
if (scaled_begin.x() > scaled_end.x()) {
//flag=1代表end和begin交换
flag=1;
return RayToPixelMaskVisualNew(scaled_end, scaled_begin, subpixel_scale,intensities,local_pose_,flag,hit_table,
probability_grid);
}
CHECK_GE(scaled_begin.x(), 0);
CHECK_GE(scaled_begin.y(), 0);
CHECK_GE(scaled_end.y(), 0);
std::vector<Eigen::Array2i> pixel_mask;
// Special case: We have to draw a vertical line in full pixels, as
// 'scaled_begin' and 'scaled_end' have the same full pixel x coordinate.
if (scaled_begin.x() / subpixel_scale == scaled_end.x() / subpixel_scale) {
Eigen::Array2i current(
scaled_begin.x() / subpixel_scale,
std::min(scaled_begin.y(), scaled_end.y()) / subpixel_scale);
pixel_mask.push_back(current);
const int end_y =
std::max(scaled_begin.y(), scaled_end.y()) / subpixel_scale;
for (; current.y() <= end_y; ++current.y()) {
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
}
const Eigen::Matrix4d homo =ToEigen(local_pose_).matrix();
// jzq新加 标记摄像头范围
if(intensities != 0) {
if ((scaled_end.x()/1000-scaled_begin.x()/1000)*(scaled_end.x()/1000-scaled_begin.x()/1000)
+(scaled_end.y()/1000-scaled_begin.y()/1000)*(scaled_end.y()/1000-scaled_begin.y()/1000)<576){
for(Eigen::Array2i pixel : pixel_mask) {
probability_grid->mutable_semantics_observer()->at(probability_grid->ToFlatIndex(pixel)) = 1;
probability_grid->mutable_scanSemantics_observer()->at(probability_grid->ToFlatIndex(pixel)) = 1;
}
}
else{
Eigen::Array2i ori_begin;
if(flag == 1)
ori_begin = scaled_end;
else
ori_begin = scaled_begin;
for(Eigen::Array2i pixel : pixel_mask){
if ((pixel.x()-ori_begin.x()/1000)*(pixel.x()-ori_begin.x()/1000)
+(pixel.y()-ori_begin.y()/1000)*(pixel.y()-ori_begin.y()/1000)<576) {
probability_grid->mutable_semantics_observer()->at(probability_grid->ToFlatIndex(pixel)) = 1;
probability_grid->mutable_scanSemantics_observer()->at(probability_grid->ToFlatIndex(pixel)) = 1;
}
}
}
}
return pixel_mask;
//return ProcessPixelMask(pixel_mask,scaled_begin,scaled_end,subpixel_scale,flag,intensities,local_pose_);
}
const int64 dx = scaled_end.x() - scaled_begin.x();
const int64 dy = scaled_end.y() - scaled_begin.y();
const int64 denominator = 2 * subpixel_scale * dx;
// The current full pixel coordinates. We scaled_begin at 'scaled_begin'.
Eigen::Array2i current = scaled_begin / subpixel_scale;
pixel_mask.push_back(current);
// To represent subpixel centers, we use a factor of 2 * 'subpixel_scale' in
// the denominator.
// +-+-+-+ -- 1 = (2 * subpixel_scale) / (2 * subpixel_scale)
// | | | |
// +-+-+-+
// | | | |
// +-+-+-+ -- top edge of first subpixel = 2 / (2 * subpixel_scale)
// | | | | -- center of first subpixel = 1 / (2 * subpixel_scale)
// +-+-+-+ -- 0 = 0 / (2 * subpixel_scale)
// The center of the subpixel part of 'scaled_begin.y()' assuming the
// 'denominator', i.e., sub_y / denominator is in (0, 1).
int64 sub_y = (2 * (scaled_begin.y() % subpixel_scale) + 1) * dx;
// The distance from the from 'scaled_begin' to the right pixel border, to be
// divided by 2 * 'subpixel_scale'.
const int first_pixel =
2 * subpixel_scale - 2 * (scaled_begin.x() % subpixel_scale) - 1;
// The same from the left pixel border to 'scaled_end'.
const int last_pixel = 2 * (scaled_end.x() % subpixel_scale) + 1;
// The full pixel x coordinate of 'scaled_end'.
const int end_x = std::max(scaled_begin.x(), scaled_end.x()) / subpixel_scale;
// Move from 'scaled_begin' to the next pixel border to the right.
sub_y += dy * first_pixel;
if (dy > 0) {
while (true) {
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
while (sub_y > denominator) {
sub_y -= denominator;
++current.y();
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
}
++current.x();
if (sub_y == denominator) {
sub_y -= denominator;
++current.y();
}
if (current.x() == end_x) {
break;
}
// Move from one pixel border to the next.
sub_y += dy * 2 * subpixel_scale;
}
// Move from the pixel border on the right to 'scaled_end'.
sub_y += dy * last_pixel;
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
while (sub_y > denominator) {
sub_y -= denominator;
++current.y();
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
}
CHECK_NE(sub_y, denominator);
CHECK_EQ(current.y(), scaled_end.y() / subpixel_scale);
// jzq新加 标记摄像头范围
if(intensities != 0) {
if ((scaled_end.x()/1000-scaled_begin.x()/1000)*(scaled_end.x()/1000-scaled_begin.x()/1000)
+(scaled_end.y()/1000-scaled_begin.y()/1000)*(scaled_end.y()/1000-scaled_begin.y()/1000)<576){
for(Eigen::Array2i pixel : pixel_mask) {
probability_grid->mutable_semantics_observer()->at(probability_grid->ToFlatIndex(pixel)) = 1;
probability_grid->mutable_scanSemantics_observer()->at(probability_grid->ToFlatIndex(pixel)) = 1;
}
}
else{
Eigen::Array2i ori_begin;
if(flag == 1)
ori_begin = scaled_end;
else
ori_begin = scaled_begin;
for(Eigen::Array2i pixel : pixel_mask){
if ((pixel.x()-ori_begin.x()/1000)*(pixel.x()-ori_begin.x()/1000)
+(pixel.y()-ori_begin.y()/1000)*(pixel.y()-ori_begin.y()/1000)<576) {
probability_grid->mutable_semantics_observer()->at(probability_grid->ToFlatIndex(pixel)) = 1;
probability_grid->mutable_scanSemantics_observer()->at(probability_grid->ToFlatIndex(pixel)) = 1;
}
}
}
}
return pixel_mask;
//可以不针对pixel_mask进行遍历，当pushback一个miss点就对其判断 --王煜坤
// return ProcessPixelMask(pixel_mask,scaled_begin,scaled_end,subpixel_scale,flag,intensities,local_pose_);
}
// Same for lines non-ascending in y coordinates.
while (true) {
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
while (sub_y < 0) {
sub_y += denominator;
--current.y();
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
}
++current.x();
if (sub_y == 0) {
sub_y += denominator;
--current.y();
}
if (current.x() == end_x) {
break;
}
sub_y += dy * 2 * subpixel_scale;
}
sub_y += dy * last_pixel;
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
while (sub_y < 0) {
sub_y += denominator;
--current.y();
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
}
CHECK_NE(sub_y, 0);
CHECK_EQ(current.y(), scaled_end.y() / subpixel_scale);
// jzq新加 标记摄像头范围
if(intensities != 0) {
if ((scaled_end.x()/1000-scaled_begin.x()/1000)*(scaled_end.x()/1000-scaled_begin.x()/1000)
+(scaled_end.y()/1000-scaled_begin.y()/1000)*(scaled_end.y()/1000-scaled_begin.y()/1000)<576){
for(Eigen::Array2i pixel : pixel_mask) {
probability_grid->mutable_semantics_observer()->at(probability_grid->ToFlatIndex(pixel)) = 1;
probability_grid->mutable_scanSemantics_observer()->at(probability_grid->ToFlatIndex(pixel)) = 1;
}
}
else{
Eigen::Array2i ori_begin;
if(flag == 1)
ori_begin = scaled_end;
else
ori_begin = scaled_begin;
for(Eigen::Array2i pixel : pixel_mask){
if ((pixel.x()-ori_begin.x()/1000)*(pixel.x()-ori_begin.x()/1000)
+(pixel.y()-ori_begin.y()/1000)*(pixel.y()-ori_begin.y()/1000)<576) {
probability_grid->mutable_semantics_observer()->at(probability_grid->ToFlatIndex(pixel)) = 1;
probability_grid->mutable_scanSemantics_observer()->at(probability_grid->ToFlatIndex(pixel)) = 1;
}
}
}
}
return pixel_mask;
// return ProcessPixelMask(pixel_mask,scaled_begin,scaled_end,subpixel_scale,flag,intensities,local_pose_);
}
std::vector<Eigen::Array2i> RayToPixelMaskVisual(const Eigen::Array2i& scaled_begin,
const Eigen::Array2i& scaled_end,
int subpixel_scale,
const uint8 intensities) {
// For simplicity, we order 'scaled_begin' and 'scaled_end' by their x
// coordinate.
if (intensities != 0){
if (scaled_begin.x() > scaled_end.x()) {
return RayToPixelMask(scaled_end, scaled_begin, subpixel_scale);
}
CHECK_GE(scaled_begin.x(), 0);
CHECK_GE(scaled_begin.y(), 0);
CHECK_GE(scaled_end.y(), 0);
std::vector<Eigen::Array2i> pixel_mask;
// Special case: We have to draw a vertical line in full pixels, as
// 'scaled_begin' and 'scaled_end' have the same full pixel x coordinate.
if (scaled_begin.x() / subpixel_scale == scaled_end.x() / subpixel_scale) {
Eigen::Array2i current(
scaled_begin.x() / subpixel_scale,
std::min(scaled_begin.y(), scaled_end.y()) / subpixel_scale);
pixel_mask.push_back(current);
const int end_y =
std::max(scaled_begin.y(), scaled_end.y()) / subpixel_scale;
for (; current.y() <= end_y; ++current.y()) {
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
}
return pixel_mask;
}
const int64 dx = scaled_end.x() - scaled_begin.x();
const int64 dy = scaled_end.y() - scaled_begin.y();
const int64 denominator = 2 * subpixel_scale * dx;
// The current full pixel coordinates. We scaled_begin at 'scaled_begin'.
Eigen::Array2i current = scaled_begin / subpixel_scale;
pixel_mask.push_back(current);
// To represent subpixel centers, we use a factor of 2 * 'subpixel_scale' in
// the denominator.
// +-+-+-+ -- 1 = (2 * subpixel_scale) / (2 * subpixel_scale)
// | | | |
// +-+-+-+
// | | | |
// +-+-+-+ -- top edge of first subpixel = 2 / (2 * subpixel_scale)
// | | | | -- center of first subpixel = 1 / (2 * subpixel_scale)
// +-+-+-+ -- 0 = 0 / (2 * subpixel_scale)
// The center of the subpixel part of 'scaled_begin.y()' assuming the
// 'denominator', i.e., sub_y / denominator is in (0, 1).
int64 sub_y = (2 * (scaled_begin.y() % subpixel_scale) + 1) * dx;
// The distance from the from 'scaled_begin' to the right pixel border, to be
// divided by 2 * 'subpixel_scale'.
const int first_pixel =
2 * subpixel_scale - 2 * (scaled_begin.x() % subpixel_scale) - 1;
// The same from the left pixel border to 'scaled_end'.
const int last_pixel = 2 * (scaled_end.x() % subpixel_scale) + 1;
// The full pixel x coordinate of 'scaled_end'.
const int end_x = std::max(scaled_begin.x(), scaled_end.x()) / subpixel_scale;
// Move from 'scaled_begin' to the next pixel border to the right.
sub_y += dy * first_pixel;
if (dy > 0) {
while (true) {
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
while (sub_y > denominator) {
sub_y -= denominator;
++current.y();
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
}
++current.x();
if (sub_y == denominator) {
sub_y -= denominator;
++current.y();
}
if (current.x() == end_x) {
break;
}
// Move from one pixel border to the next.
sub_y += dy * 2 * subpixel_scale;
}
// Move from the pixel border on the right to 'scaled_end'.
sub_y += dy * last_pixel;
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
while (sub_y > denominator) {
sub_y -= denominator;
++current.y();
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
}
CHECK_NE(sub_y, denominator);
CHECK_EQ(current.y(), scaled_end.y() / subpixel_scale);
return pixel_mask;
}
// Same for lines non-ascending in y coordinates.
while (true) {
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
while (sub_y < 0) {
sub_y += denominator;
--current.y();
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
}
++current.x();
if (sub_y == 0) {
sub_y += denominator;
--current.y();
}
if (current.x() == end_x) {
break;
}
sub_y += dy * 2 * subpixel_scale;
}
sub_y += dy * last_pixel;
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
while (sub_y < 0) {
sub_y += denominator;
--current.y();
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
}
CHECK_NE(sub_y, 0);
CHECK_EQ(current.y(), scaled_end.y() / subpixel_scale);
return pixel_mask;
}else {
return std::vector<Eigen::Array2i> ();
}
}
} // namespace mapping
} // namespace cartographer