No distortion_model and D param in /pepper_robot/camera/depth/camera_info

[robograffitti]

The values for distortion_model and D is blank in /pepper_robot/camera/depth/camera_info like below:

distortion_model:
D:

These two are supposed to be:
distortion_model: plumb_bob
D: [0.0, 0.0, 0.0, 0.0, 0.0]

Is it difficult to publish these values?
Because of this, I can not use depthimage_to_laserscan.

[Karsten1987]

Hi,

I am not sure why, but exactly these two lines for the distortion model are commented in https://github.com/ros-naoqi/naoqi_driver/blob/master/src/converters/camera_info_definitions.hpp#L176
If you can, just uncomment them, recompile and check if you succeed in transforming into a laserscan.

This can definitely be a PR.

[robograffitti]

Thank you so much!

I uncommented the 6 lines for the distortion and recompiled it. Then it finally worked!

[vrabaud]

@yoshimalucky , any news on that ? thx.

[dac31415]

After I uncomment and tried to recompile, the following mistake arises.
/opt/ros/melodic/include/qi/detail/future_fwd.hpp:750:5: note: declared here
operator const typename Future::ValueTypeCast&() const { _sync = false; return _future.value(); }
^~~~~~~~
naoqi_driver/CMakeFiles/naoqi_driver.dir/build.make:230: recipe for target 'naoqi_driver/CMakeFiles/naoqi_driver.dir/src/converters/camera.cpp.o' failed
make[2]: *** [naoqi_driver/CMakeFiles/naoqi_driver.dir/src/converters/camera.cpp.o] Error 1
CMakeFiles/Makefile2:9042: recipe for target 'naoqi_driver/CMakeFiles/naoqi_driver.dir/all' failed
make[1]: *** [naoqi_driver/CMakeFiles/naoqi_driver.dir/all] Error 2
Makefile:140: recipe for target 'all' failed
make: *** [all] Error 2
Invoking "make -j4 -l4" failed

Any reasons why?

[mbusy]

Since you're using melodic, my guess would be that it's a boost related issue. Take a look at this version of the file here (softbank robotics research's fork), this should allow you to get rid of boost for that file, and include the distortion model. What we use in the file is pretty old (for retro-compatibility purposes), you could consider using the functionalities of the standard lib instead.

[dac31415]

Thanks again. @mbusy I did change it. No error but the issue is still the same.

[mbusy]

@dac31415, that's weird, the distortion model and D should be specified in the camera_info (checkout these lines, that's for the VGA quality but it's also enabled for QVGA and QQVGA)

How did you launch the driver ?

[dac31415]

@mbusy the problem might be when with the pepper bringup package.
I executed the driver:
roslaunch naoqi_driver naoqi_driver.launch naop_ip:=<nao_ip> network_interface:=<networ...>
after that I can view (either with rviz or rqt_image_view) all the raw images from the cameras with the topics
/naoqi_driver/camera/.../image_raw
and also execute teleop with roslaunch nao_teleop teleop_joy.launch

[mbusy]

Thanks @dac31415. We probably won't be investigating the errors in the pepper bringup package, we want to focus the maintaining efforts on the driver. That being said, PRs would definitely be considered / reviewed / merged

[dac31415]

Ok, thanks @mbusy. I'll keep looking on how to solve it. The naoqi_driver is working fine for image viewing but is when I try to convert the data from the cameras. (depth to point cloud) as I haven't been able to do it. So I don't know if it's the pepper_bringup or I am missing some other dependencies or packages.

[UdayRockzz]

Hi @dac31415 , Did you solve this issue(depth to point cloud)?

[dac31415]

@UdayRockzz follow steps from @mbusy (softbank robotics research's fork)

[UdayRockzz]

@dac31415 Thanks a lot for responding. I have followed all those steps which @mbusy suggested but still, I am unable to get the point cloud information. If it is possible could you please share the file with me? It will be helpful for my analysis.

[redsIng]

I have exactly the same problem. In addition I have uncommented those lines in camer_info_definitions.hpp and corrected the resulting error adding ".convert_to_container<std::vector >();" to all cam_info_msg.D lines like how it has been done for front and bottom camera in the same file. I compile it and all seems work. When I do depth camera calibration following Pepper/Tutorial/Calibration guide to generate .yaml file and then use depthimage_to_laserscan ros package, the front and bottom camera works but the depth camera can't calibrate so I can't use depthimage_to_laserscan. Can you provide me any suggestion on how fix it? Am I doing it wrong?
Thanks in advance.

This is my camera_info_definitions.hpp without the correction that I did:
`
#ifndef CAMERA_INFO_DEF_HPP
#define CAMERA_INFO_DEF_HPP

#include <sensor_msgs/CameraInfo.h>
#include <boost/assign/list_of.hpp>

namespace naoqi
{
namespace converter
{
namespace camera_info_definitions
{

/**

• TOP CAMERA
  */
  inline sensor_msgs::CameraInfo createCameraInfoTOPVGA()
  {
  sensor_msgs::CameraInfo cam_info_msg;

  cam_info_msg.header.frame_id = "CameraTop_optical_frame";

  cam_info_msg.width = 640;
  cam_info_msg.height = 480;
  cam_info_msg.K = boost::array<double, 9>{{ 556.845054830986, 0, 309.366895338178, 0, 555.898679730161, 230.592233628776, 0, 0, 1 }};

  cam_info_msg.distortion_model = "plumb_bob";
  cam_info_msg.D = boost::assign::list_of(-0.0545211535376379)(0.0691973423510287)(-0.00241094929163055)(-0.00112245009306511)(0).convert_to_container<std::vector >();

  cam_info_msg.R = boost::array<double, 9>{{ 1, 0, 0, 0, 1, 0, 0, 0, 1 }};

  cam_info_msg.P = boost::array<double, 12>{{ 551.589721679688, 0, 308.271132841983, 0, 0, 550.291320800781, 229.20143668168, 0, 0, 0, 1, 0 }};

  return cam_info_msg;
  }

inline sensor_msgs::CameraInfo createCameraInfoTOPQVGA()
{
sensor_msgs::CameraInfo cam_info_msg;

cam_info_msg.header.frame_id = "CameraTop_optical_frame";

cam_info_msg.width = 320;
cam_info_msg.height = 240;
cam_info_msg.K = boost::array<double, 9>{{ 274.139508945831, 0, 141.184472810944, 0, 275.741846757374, 106.693773654172, 0, 0, 1 }};

cam_info_msg.distortion_model = "plumb_bob";
cam_info_msg.D = boost::assign::list_of(-0.0870160932911717)(0.128210165050533)(0.003379500659424)(-0.00106205540818586)(0).convert_to_container<std::vector >();

cam_info_msg.R = boost::array<double, 9>{{ 1, 0, 0, 0, 1, 0, 0, 0, 1 }};

cam_info_msg.P = boost::array<double, 12>{{ 272.423675537109, 0, 141.131930791285, 0, 0, 273.515747070312, 107.391746054313, 0, 0, 0, 1, 0 }};

return cam_info_msg;
}

inline sensor_msgs::CameraInfo createCameraInfoTOPQQVGA()
{
sensor_msgs::CameraInfo cam_info_msg;

cam_info_msg.header.frame_id = "CameraTop_optical_frame";

cam_info_msg.width = 160;
cam_info_msg.height = 120;
cam_info_msg.K = boost::array<double, 9>{{ 139.424539568966, 0, 76.9073669920582, 0, 139.25542782325, 59.5554242026743, 0, 0, 1 }};

cam_info_msg.distortion_model = "plumb_bob";
cam_info_msg.D = boost::assign::list_of(-0.0843564504845967)(0.125733083790192)(0.00275901756247071)(-0.00138645823460527)(0).convert_to_container<std::vector >();

cam_info_msg.R = boost::array<double, 9>{{ 1, 0, 0, 0, 1, 0, 0, 0, 1 }};

cam_info_msg.P = boost::array<double, 12>{{ 137.541534423828, 0, 76.3004646597892, 0, 0, 136.815216064453, 59.3909799751191, 0, 0, 0, 1, 0 }};

return cam_info_msg;
}

/**

• BOTTOM CAMERA
  */
  inline sensor_msgs::CameraInfo createCameraInfoBOTTOMVGA()
  {
  sensor_msgs::CameraInfo cam_info_msg;

  cam_info_msg.header.frame_id = "CameraBottom_optical_frame";

  cam_info_msg.width = 640;
  cam_info_msg.height = 480;
  cam_info_msg.K = boost::array<double, 9>{{ 558.570339530768, 0, 308.885375457296, 0, 556.122943034837, 247.600724811385, 0, 0, 1 }};

  cam_info_msg.distortion_model = "plumb_bob";
  cam_info_msg.D = boost::assign::list_of(-0.0648763971625288)(0.0612520196884308)(0.0038281538281731)(-0.00551104078371959)(0).convert_to_container<std::vector >();

  cam_info_msg.R = boost::array<double, 9>{{ 1, 0, 0, 0, 1, 0, 0, 0, 1 }};

  cam_info_msg.P = boost::array<double, 12>{{ 549.571655273438, 0, 304.799679526441, 0, 0, 549.687316894531, 248.526959297022, 0, 0, 0, 1, 0 }};

  return cam_info_msg;
  }

inline sensor_msgs::CameraInfo createCameraInfoBOTTOMQVGA()
{
sensor_msgs::CameraInfo cam_info_msg;

cam_info_msg.header.frame_id = "CameraBottom_optical_frame";

cam_info_msg.width = 320;
cam_info_msg.height = 240;
cam_info_msg.K = boost::array<double, 9>{{ 278.236008818534, 0, 156.194471689706, 0, 279.380102992049, 126.007123836447, 0, 0, 1 }};

cam_info_msg.distortion_model = "plumb_bob";
cam_info_msg.D = boost::assign::list_of(-0.0481869853715082)(0.0201858398559121)(0.0030362056699177)(-0.00172241952442813)(0).convert_to_container<std::vector >();

cam_info_msg.R = boost::array<double, 9>{{ 1, 0, 0, 0, 1, 0, 0, 0, 1 }};

cam_info_msg.P = boost::array<double, 12>{{ 273.491455078125, 0, 155.112454709117, 0, 0, 275.743133544922, 126.057357467223, 0, 0, 0, 1, 0 }};

return cam_info_msg;
}

inline sensor_msgs::CameraInfo createCameraInfoBOTTOMQQVGA()
{
sensor_msgs::CameraInfo cam_info_msg;

cam_info_msg.header.frame_id = "CameraBottom_optical_frame";

cam_info_msg.width = 160;
cam_info_msg.height = 120;
cam_info_msg.K = boost::array<double, 9>{{ 141.611855886672, 0, 78.6494086288656, 0, 141.367163830175, 58.9220646201529, 0, 0, 1 }};

cam_info_msg.distortion_model = "plumb_bob";
cam_info_msg.D = boost::assign::list_of(-0.0688388724945936)(0.0697453843669642)(0.00309518737071049)(-0.00570486993696543)(0).convert_to_container<std::vector >();

cam_info_msg.R = boost::array<double, 9>{{ 1, 0, 0, 0, 1, 0, 0, 0, 1 }};

cam_info_msg.P = boost::array<double, 12>{{ 138.705535888672, 0, 77.2544255212306, 0, 0, 138.954086303711, 58.7000861760043, 0, 0, 0, 1, 0 }};

return cam_info_msg;
}

/**

• DEPTH CAMERA
  */
  inline sensor_msgs::CameraInfo createCameraInfoDEPTHVGA()
  {
  sensor_msgs::CameraInfo cam_info_msg;

  cam_info_msg.header.frame_id = "CameraDepth_optical_frame";

  cam_info_msg.width = 640;
  cam_info_msg.height = 480;
  cam_info_msg.K = boost::array<double, 9>{{ 525, 0, 319.5000000, 0, 525, 239.5000000000000, 0, 0, 1 }};

  cam_info_msg.distortion_model = "plumb_bob";
  //cam_info_msg.D = boost::assign::list_of(-0.0688388724945936)(0.0697453843669642)(0.00309518737071049)(-0.00570486993696543)(0);

  cam_info_msg.R = boost::array<double, 9>{{ 1, 0, 0, 0, 1, 0, 0, 0, 1 }};

  cam_info_msg.P = boost::array<double, 12>{{ 525, 0, 319.500000, 0, 0, 525, 239.5000000000, 0, 0, 0, 1, 0 }};

  return cam_info_msg;
  }

inline sensor_msgs::CameraInfo createCameraInfoDEPTHQVGA()
{
sensor_msgs::CameraInfo cam_info_msg;

cam_info_msg.header.frame_id = "CameraDepth_optical_frame";

cam_info_msg.width = 320;
cam_info_msg.height = 240;
cam_info_msg.K = boost::array<double, 9>{{ 525/2.0f, 0, 319.5000000/2.0f, 0, 525/2.0f, 239.5000000000000/2.0f, 0, 0, 1 }};

//cam_info_msg.distortion_model = "plumb_bob";
//cam_info_msg.D = boost::assign::list_of(-0.0688388724945936)(0.0697453843669642)(0.00309518737071049)(-0.00570486993696543)(0);

cam_info_msg.R = boost::array<double, 9>{{ 1, 0, 0, 0, 1, 0, 0, 0, 1 }};

cam_info_msg.P = boost::array<double, 12>{{ 525/2.0f, 0, 319.500000/2.0f, 0, 0, 525/2.0f, 239.5000000000/2.0f, 0, 0, 0, 1, 0 }};

return cam_info_msg;
}

inline sensor_msgs::CameraInfo createCameraInfoDEPTHQQVGA()
{
sensor_msgs::CameraInfo cam_info_msg;

cam_info_msg.header.frame_id = "CameraDepth_optical_frame";

cam_info_msg.width = 160;
cam_info_msg.height = 120;
cam_info_msg.K = boost::array<double, 9>{{ 525/4.0f, 0, 319.5000000/4.0f, 0, 525/4.0f, 239.5000000000000/4.0f, 0, 0, 1 }};

//cam_info_msg.distortion_model = "plumb_bob";
//cam_info_msg.D = boost::assign::list_of(-0.0688388724945936)(0.0697453843669642)(0.00309518737071049)(-0.00570486993696543)(0);

cam_info_msg.R = boost::array<double, 9>{{ 1, 0, 0, 0, 1, 0, 0, 0, 1 }};

cam_info_msg.P = boost::array<double, 12>{{ 525/4.0f, 0, 319.500000/4.0f, 0, 0, 525/4.0f, 239.5000000000/4.0f, 0, 0, 0, 1, 0 }};

return cam_info_msg;
}

/**

• STEREO CAMERA
  */
  inline sensor_msgs::CameraInfo createCameraInfoStereo(
  const int &width,
  const int &height,
  const float &reductionFactor) {

  sensor_msgs::CameraInfo cam_info_msg;

  cam_info_msg.header.frame_id = "CameraDepth_optical_frame";

  const size_t nK = 9;
  const size_t nD = 5;
  const size_t nR = 9;
  const size_t nP = 12;

  float kTab[nK] = {703.102356f/reductionFactor, 0, 647.821594f/reductionFactor,
  0, 702.432312f/reductionFactor, 380.971680f/reductionFactor,
  0, 0, 1 };

  float dTab[nD] = {-0.168594331,
  .00881872326,
  -0.000182721298,
  -0.0000145479062,
  0.0137237618};

  float rTab[nR] = {0.999984741, 0.000130843779, 0.00552622462,
  -0.000111592424, 0.999993920, -0.00348380185,
  -0.00552664697, 0.00348313176, 0.999978662};

  float pTab[nP] = {569.869568f/reductionFactor, 0, 644.672058f/reductionFactor, 0,
  0, 569.869568f/reductionFactor, 393.368958f/reductionFactor, 0,
  0, 0, 1, 0 };

  cam_info_msg.width = width;
  cam_info_msg.height = height;

  for (int i = 0; i < nK; ++i)
  cam_info_msg.K.at(i) = kTab[i];

  cam_info_msg.distortion_model = "plumb_bob";
  cam_info_msg.D.assign(dTab, dTab + nD);

  for (int i = 0; i < nR; ++i)
  cam_info_msg.R.at(i) = rTab[i];

  for (int i = 0; i < nP; ++i)
  cam_info_msg.P.at(i) = pTab[i];

  return cam_info_msg;
  }

inline sensor_msgs::CameraInfo createCameraInfoDEPTH720P()
{
return createCameraInfoStereo(1280, 720, 1.0);
}

inline sensor_msgs::CameraInfo createCameraInfoDEPTHQ720P()
{
return createCameraInfoStereo(640, 360, 2.0);
}

inline sensor_msgs::CameraInfo createCameraInfoDEPTHQQ720P()
{
return createCameraInfoStereo(320, 180, 4.0);
}

inline sensor_msgs::CameraInfo createCameraInfoDEPTHQQQ720P()
{
return createCameraInfoStereo(160, 90, 8.0);
}

inline sensor_msgs::CameraInfo createCameraInfoDEPTHQQQQ720P()
{
return createCameraInfoStereo(80, 45, 16.0);
}

// Complete methods for stereo image parameteres
inline sensor_msgs::CameraInfo createCameraInfoStereo720PX2()
{
return createCameraInfoStereo(2560, 720, 1.0);
}

inline sensor_msgs::CameraInfo createCameraInfoStereoQ720PX2()
{
return createCameraInfoStereo(1280, 360, 2.0);
}

inline sensor_msgs::CameraInfo createCameraInfoStereoQQ720PX2()
{
return createCameraInfoStereo(640, 180, 4.0);
}

inline sensor_msgs::CameraInfo createCameraInfoStereoQQQ720PX2()
{
return createCameraInfoStereo(320, 90, 8.0);
}

inline sensor_msgs::CameraInfo createCameraInfoStereoQQQQ720PX2()
{
return createCameraInfoStereo(160, 45, 16.0);
}

} // camera_info_definitions
} //publisher
} //naoqi

#endif
`