Problem about `distance_from_origin`

[Nothand0212]

when i capture a sample, it shows:

Measured board has: dimensions = 576.02389 x 989.93934 mm; area = 0.56602m^2
Distance =  0.00 m
Board angles     = 96.97,89.20,90.47,98.25 degrees
Board area       = 0.57054 m^2 (+0.00452 m^2)
Board avg height = 989.94mm (+9.07mm)
Board avg width  = 576.02mm (-1.03mm)
Board dim        = 567.52,953.30,1026.57,584.53 mm
Board dim error  =   90.28
Avg Lidar Normal =  -0.15,  -0.99,   0.04 
Avg Lidar Centre = 545.69, 2175.47, -74.88 
Std Dev Lidar Normal =   0.00,   0.00,   0.00 
Std Dev Lidar Centre =   3.00,   1.57,   1.53 

the Distance is always 0.

when I look up the code, it seems bug

          // Board dimension errors
          double w0_diff    = abs( avg_w0 - board_width_ );
          double w1_diff    = abs( avg_w1 - board_width_ );
          double h0_diff    = abs( avg_h0 - board_height_ );
          double h1_diff    = abs( avg_h1 - board_height_ );
          double be_dim_err = w0_diff + w1_diff + h0_diff + h1_diff;

          double distance = sqrt( pow( sample.lidar_centre.x / 1000 - 0, 2 ) + pow( sample.lidar_centre.y / 1000 - 0, 2 ) +
                                  pow( sample.lidar_centre.z / 1000 - 0, 2 ) );

          ROS_INFO_STREAM( "On Line: " << __LINE__ << " Distance: " << distance << " m" );
          ROS_INFO_STREAM( "On Line: " << __LINE__ << " Lidar Centre: " << sample.lidar_centre.x << ", " << sample.lidar_centre.y << ", " << sample.lidar_centre.z );

          sample.camera_centre = cv::Point3d( sum_camera_centre.x / samples_.size(), sum_camera_centre.y / samples_.size(),
                                              sum_camera_centre.z / samples_.size() );

          sample.camera_normal = cv::Point3d( sum_camera_normal.x / samples_.size(), sum_camera_normal.y / samples_.size(),
                                              sum_camera_normal.z / samples_.size() );

          sample.camera_corners = { avg_camera_c0, avg_camera_c1, avg_camera_c2, avg_camera_c3 };

          sample.lidar_centre = cv::Point3d( sum_lidar_centre.x / samples_.size(), sum_lidar_centre.y / samples_.size(),
                                             sum_lidar_centre.z / samples_.size() );
          ROS_INFO_STREAM( "On Line: " << __LINE__ << " Distance: " << distance << " m" );
          ROS_INFO_STREAM( "On Line: " << __LINE__ << " Lidar Centre: " << sum_lidar_centre.x / samples_.size() << ", " << sum_lidar_centre.y / samples_.size() << ", " << sum_lidar_centre.z / samples_.size() );

          sample.lidar_normal = cv::Point3d( sum_lidar_normal.x / samples_.size(), sum_lidar_normal.y / samples_.size(), sum_lidar_normal.z / samples_.size() );

the variable distance is calculated with sample.lidar_centre, but it it update below, that cause distance always zero

[jclinton830]

@Nothand0212 thanks for that. We accept PRs if you can fix it please contribute back and we will review it and merge into the master

[jclinton830]

Thanks again @Nothand0212 for your PR #56