Method convertToWorldFrame

opt_calibration/src/opt_calibration.cpp:366–416

}

void OPTCalibration::convertToWorldFrame()
{
  for (size_t i = 0; i < node_vec_.size(); ++i)
  {
    const TreeNode & sensor_node = *node_vec_[i];
    cb::Pose pose = sensor_node.sensor()->pose();
    cb::BaseObject::ConstPtr parent = sensor_node.sensor()->parent();

    while (parent->parent())
    {
      pose = parent->pose() * pose;
      parent = parent->parent();
      sensor_node.sensor()->setParent(parent);
      sensor_node.sensor()->setPose(pose);
    }

    //ROS_INFO_STREAM(std::endl << sensor_node.sensor()->pose().matrix());
  }

  for (size_t i = checkerboard_vec_.size(); i < view_map_vec_.size(); ++i)
  {
    ViewMap & view_map = view_map_vec_[i];

    ViewMap::iterator it = view_map.begin();
    bool ok = false;
    while (not ok and it != view_map.end())
    {
      if (it->first->type() == TreeNode::DEPTH)
        ++it;
      else
        ok = true;
    }

    const CheckerboardView::Ptr & cb_view = it->second;
    const TreeNode::Ptr & sensor_node = it->first;

    cb::Checkerboard::Ptr cb = boost::make_shared<cb::Checkerboard>(*boost::static_pointer_cast<cb::Checkerboard>(cb_view->object));

    cb::BaseObject::ConstPtr parent = sensor_node->sensor();
    while (parent->parent())
    {
      cb->transform(parent->pose());
      parent = parent->parent();
    }
    cb->setParent(parent);

    checkerboard_vec_.push_back(cb);
    is_floor_vec_.push_back(cb_view->is_floor);

  }
}

class RootPinholeError
{