<commit_before>#include <stdio.h>
#include <string.h>
#include <iostream>
#include <sstream>
#include <sys/time.h>
#include <stdint.h>
#include "modp_b64.h"

using namespace std;
const int LOOP_COUNT = 100000;

static const uint8_t kBase64DecodeTable[256] ={
0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,
0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,
0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0x3e,0xff,0xff,0xff,0x3f,
0x34,0x35,0x36,0x37,0x38,0x39,0x3a,0x3b,0x3c,0x3d,0xff,0xff,0xff,0xff,0xff,0xff,
0xff,0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08,0x09,0x0a,0x0b,0x0c,0x0d,0x0e,
0x0f,0x10,0x11,0x12,0x13,0x14,0x15,0x16,0x17,0x18,0x19,0xff,0xff,0xff,0xff,0xff,
0xff,0x1a,0x1b,0x1c,0x1d,0x1e,0x1f,0x20,0x21,0x22,0x23,0x24,0x25,0x26,0x27,0x28,
0x29,0x2a,0x2b,0x2c,0x2d,0x2e,0x2f,0x30,0x31,0x32,0x33,0xff,0xff,0xff,0xff,0xff,
0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,
0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,
0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,
0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,
0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,
0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,
0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,
0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,
};
static const uint8_t *kBase64EncodeTable = (const uint8_t *)
  "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";

void  base64_encode(const uint8_t *in, uint32_t len, uint8_t *buf) {
  buf[0] = kBase64EncodeTable[(in[0] >> 2) & 0x3f];
  if (len == 3) {
    buf[1] = kBase64EncodeTable[((in[0] << 4) & 0x30) | ((in[1] >> 4) & 0x0f)];
    buf[2] = kBase64EncodeTable[((in[1] << 2) & 0x3c) | ((in[2] >> 6) & 0x03)];
    buf[3] = kBase64EncodeTable[in[2] & 0x3f];
  } else if (len == 2) {
    buf[1] = kBase64EncodeTable[((in[0] << 4) & 0x30) | ((in[1] >> 4) & 0x0f)];
    buf[2] = kBase64EncodeTable[(in[1] << 2) & 0x3c];
  } else  { // len == 1
    buf[1] = kBase64EncodeTable[(in[0] << 4) & 0x30];
  }
}

std::string Base64Encode(const std::string& src, std::string ending = "")
{
  uint8_t i = 0;
  uint8_t b[4];
  const uint8_t *bytes = (const uint8_t *) src.c_str();
  uint32_t len = src.length();

  std::stringstream ss;

  while (len >= 3)
  {
    base64_encode(bytes, 3, b);
    for (i = 0; i < 4; i++)
      ss << b[i];
    bytes += 3;
    len -= 3;
  }

  if (len > 0)
  {
    base64_encode(bytes, len, b);
    for (i = 0; i < len + 1; i++)
      ss << b[i];
  }

  ss << ending;

  return ss.str();
}

std::string Base64Encode1(const std::string& src, std::string ending = "")
{
  uint8_t i = 0;
  uint8_t b[4];
  const uint8_t *bytes = (const uint8_t *) src.c_str();
  uint32_t len = src.length();

  std::string s;

  while (len >= 3)
  {
    base64_encode(bytes, 3, b);
    for (i = 0; i < 4; i++)
      s += b[i];
    bytes += 3;
    len -= 3;
  }

  if (len > 0)
  {
    base64_encode(bytes, len, b);
    for (i = 0; i < len + 1; i++)
      s += b[i];
  }

  s += ending;

  return s;
}

std::string Base64Encode2(const std::string& src, std::string ending = "")
{
  uint8_t i = 0;
  uint8_t b[4];
  const uint8_t *bytes = (const uint8_t *) src.c_str();
  uint32_t len = src.length();

  std::string s;
  s.reserve(len*4/3 + 4);
  std::cout << "begin:" << s.capacity() << std::endl;

  while (len >= 3)
  {
    base64_encode(bytes, 3, b);
    //for (i = 0; i < 4; i++)
    //s.append(b[0], 3);
    s.append(reinterpret_cast<char const*>(b),4);
    bytes += 3;
    len -= 3;
  }

  if (len)
  {
    base64_encode(bytes, len, b);
    s.append(reinterpret_cast<char const*>(b),len+1);
  }

  s += ending;
  std::cout << "end:" << s.capacity() << std::endl;

  return s;
}

std::string Base64Encode3(const std::string& src, std::string ending = "")
{
  uint8_t i = 0;
  uint8_t b[4];
  const uint8_t *bytes = (const uint8_t *) src.c_str();
  uint32_t len = src.length();

  std::string s;

  while (len >= 3)
  {
    base64_encode(bytes, 3, b);
    s += b[0];
    s += b[1];
    s += b[2];
    s += b[3];
    bytes += 3;
    len -= 3;
  }

  if (len > 0)
  {
    base64_encode(bytes, len, b);
    s.append(reinterpret_cast<char const*>(b),len+1);
  }

  s += ending;

  return s;
}

bool Base64EncodeModp(const std::string& input, std::string* output)
{
  std::string temp;
  temp.resize(modp_b64_encode_len(input.size()));  // makes room for null byte

  // null terminates result since result is base64 text!
  int input_size = static_cast<int>(input.size());
  int output_size= modp_b64_encode(&(temp[0]), input.data(), input_size);
  if (output_size < 0)
    return false;

  temp.resize(output_size);  // strips off null byte
  output->swap(temp);
  return true;
}

int main(int argc, char *argv[])
{
  std::string s = "zqus387489tid'23	tck=e02fce03a933b5c8fffdf46442a2e8e9ts"
    "1461052882959uunid4tid=23&tck=e02fce03a933b5c8fffdf46442a2e8e9&ts=1461052882959&ver=1ip:106.39.88.82hostname:tf41dg.prod.mediav.comagent:Mozilla/5.0 (Macintosh; Intel Mac OS X 10_11_4)"
    "AppleWebKit/537.36 (KHTML, like Gecko) Chrome/49.0.2623.110 Safari/537.36lang:en,zh-CN;q=0.8,zh;q=0.6,zh-TW;q=0.4referer:http://ckmap.mediav.com/b?type=10proxy:106.39.88.82, 202.79.203.111_mvctn191270=_mvsrc=118216_523698_1043440&_mvcam=191270_1241961_11970810_58740969_0&osr=oqdT0qhmgzj0&time=1449650466&rdom=anonymous; _mvctn165564=_mvsrc=118478_524001_1044112&_mvcam=165564_1224948_11775153_58841999_0&osr=Zg4c0cS49V00&time=1451040575&rdom=anonymous; v=)he((]^XdqB2fOE=[AjT; _jzqa=1.4336274511317232000.1447337529.1447337529.1461045516.2; _jzqc=1; _jzqckmp=1; ckmts=PUPtXAzi,P6PtXAzi,RGPtXAzi,R6PtXAzi,U6PtXAzi,JGPtXAzi,JrPtXAzi,J6PtXAzi,bUPtXAziver10:0|0:0|0:0|0:10p!"
    "version1.01";
  string a;
  struct timeval start, end;
  gettimeofday(&start, NULL);
  for (int i = 0; i < LOOP_COUNT; ++i)
  {
    a = Base64Encode(s);
  }
  std::cout << "base64encoded: " << a << std::endl;
  gettimeofday(&end, NULL);
  std::cout << "Base64Encode timeuse: " << 1000000*(end.tv_sec - start.tv_sec) + (end.tv_usec - start.tv_usec) << std::endl;

  string b;
  gettimeofday(&start, NULL);
  for (int i = 0; i < LOOP_COUNT; ++i)
  {
    b = Base64Encode1(s);
  }
  std::cout << "base64encoded: " << b << std::endl;
  gettimeofday(&end, NULL);
  std::cout << " Base64Encode1 timeuse: " << 1000000*(end.tv_sec - start.tv_sec) + (end.tv_usec - start.tv_usec) << std::endl;

  string e;
  gettimeofday(&start, NULL);
  for (int i = 0; i < LOOP_COUNT; ++i)
  {
    e = Base64Encode2(s);
  }
  std::cout << "base64encoded: " << e << std::endl;
  gettimeofday(&end, NULL);
  std::cout << " Base64Encode2 timeuse: " << 1000000*(end.tv_sec - start.tv_sec) + (end.tv_usec - start.tv_usec) << std::endl;

  string f;
  gettimeofday(&start, NULL);
  for (int i = 0; i < LOOP_COUNT; ++i)
  {
    f = Base64Encode3(s);
  }
  std::cout << "base64encoded: " << e << std::endl;
  gettimeofday(&end, NULL);
  std::cout << " Base64Encode3 timeuse: " << 1000000*(end.tv_sec - start.tv_sec) + (end.tv_usec - start.tv_usec) << std::endl;

  string c;
  gettimeofday(&start, NULL);
  for (int i = 0; i < LOOP_COUNT; ++i)
  {
    Base64EncodeModp(s, &c);
  }
  std::cout << "base64encoded: " << c << std::endl;
  gettimeofday(&end, NULL);
  std::cout << " Base64Encode1 timeuse: " << 1000000*(end.tv_sec - start.tv_sec) + (end.tv_usec - start.tv_usec) << std::endl;
  return 0;
}
<commit_msg>temp commit<commit_after>#include <stdio.h>
#include <string.h>
#include <iostream>
#include <sstream>
#include <sys/time.h>
#include <stdint.h>
#include "modp_b64.h"

using namespace std;
const int LOOP_COUNT = 100000;

static const uint8_t kBase64DecodeTable[256] ={
0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,
0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,
0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0x3e,0xff,0xff,0xff,0x3f,
0x34,0x35,0x36,0x37,0x38,0x39,0x3a,0x3b,0x3c,0x3d,0xff,0xff,0xff,0xff,0xff,0xff,
0xff,0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08,0x09,0x0a,0x0b,0x0c,0x0d,0x0e,
0x0f,0x10,0x11,0x12,0x13,0x14,0x15,0x16,0x17,0x18,0x19,0xff,0xff,0xff,0xff,0xff,
0xff,0x1a,0x1b,0x1c,0x1d,0x1e,0x1f,0x20,0x21,0x22,0x23,0x24,0x25,0x26,0x27,0x28,
0x29,0x2a,0x2b,0x2c,0x2d,0x2e,0x2f,0x30,0x31,0x32,0x33,0xff,0xff,0xff,0xff,0xff,
0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,
0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,
0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,
0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,
0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,
0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,
0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,
0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,
};
static const uint8_t *kBase64EncodeTable = (const uint8_t *)
  "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";

void  base64_encode(const uint8_t *in, uint32_t len, uint8_t *buf) {
  buf[0] = kBase64EncodeTable[(in[0] >> 2) & 0x3f];
  if (len == 3) {
    buf[1] = kBase64EncodeTable[((in[0] << 4) & 0x30) | ((in[1] >> 4) & 0x0f)];
    buf[2] = kBase64EncodeTable[((in[1] << 2) & 0x3c) | ((in[2] >> 6) & 0x03)];
    buf[3] = kBase64EncodeTable[in[2] & 0x3f];
  } else if (len == 2) {
    buf[1] = kBase64EncodeTable[((in[0] << 4) & 0x30) | ((in[1] >> 4) & 0x0f)];
    buf[2] = kBase64EncodeTable[(in[1] << 2) & 0x3c];
  } else  { // len == 1
    buf[1] = kBase64EncodeTable[(in[0] << 4) & 0x30];
  }
}

std::string Base64Encode(const std::string& src, std::string ending = "")
{
  uint8_t i = 0;
  uint8_t b[4];
  const uint8_t *bytes = (const uint8_t *) src.c_str();
  uint32_t len = src.length();

  std::stringstream ss;

  while (len >= 3)
  {
    base64_encode(bytes, 3, b);
    for (i = 0; i < 4; i++)
      ss << b[i];
    bytes += 3;
    len -= 3;
  }

  if (len > 0)
  {
    base64_encode(bytes, len, b);
    for (i = 0; i < len + 1; i++)
      ss << b[i];
  }

  ss << ending;

  return ss.str();
}

std::string Base64Encode1(const std::string& src, std::string ending = "")
{
  uint8_t i = 0;
  uint8_t b[4];
  const uint8_t *bytes = (const uint8_t *) src.c_str();
  uint32_t len = src.length();

  std::string s;

  while (len >= 3)
  {
    base64_encode(bytes, 3, b);
    for (i = 0; i < 4; i++)
      s += b[i];
    bytes += 3;
    len -= 3;
  }

  if (len > 0)
  {
    base64_encode(bytes, len, b);
    for (i = 0; i < len + 1; i++)
      s += b[i];
  }

  s += ending;

  return s;
}

std::string Base64Encode2(const std::string& src, std::string ending = "")
{
  uint8_t i = 0;
  uint8_t b[4];
  const uint8_t *bytes = (const uint8_t *) src.c_str();
  uint32_t len = src.length();

  std::string s;
  s.reserve(len*4/3 + 4);

  while (len >= 3)
  {
    base64_encode(bytes, 3, b);
    //for (i = 0; i < 4; i++)
    //s.append(b[0], 3);
    s.append(reinterpret_cast<char const*>(b),4);
    bytes += 3;
    len -= 3;
  }

  if (len)
  {
    base64_encode(bytes, len, b);
    s.append(reinterpret_cast<char const*>(b),len+1);
  }

  s += ending;

  return s;
}

std::string Base64Encode3(const std::string& src, std::string ending = "")
{
  uint8_t i = 0;
  uint8_t b[4];
  const uint8_t *bytes = (const uint8_t *) src.c_str();
  uint32_t len = src.length();

  std::string s;

  while (len >= 3)
  {
    base64_encode(bytes, 3, b);
    s += b[0];
    s += b[1];
    s += b[2];
    s += b[3];
    bytes += 3;
    len -= 3;
  }

  if (len > 0)
  {
    base64_encode(bytes, len, b);
    s.append(reinterpret_cast<char const*>(b),len+1);
  }

  s += ending;

  return s;
}

bool Base64EncodeModp(const std::string& input, std::string* output)
{
  std::string temp;
  temp.resize(modp_b64_encode_len(input.size()));  // makes room for null byte

  // null terminates result since result is base64 text!
  int input_size = static_cast<int>(input.size());
  int output_size= modp_b64_encode(&(temp[0]), input.data(), input_size);
  if (output_size < 0)
    return false;

  temp.resize(output_size);  // strips off null byte
  output->swap(temp);
  return true;
}

int main(int argc, char *argv[])
{
  std::string s = "zqus387489tid'23	tck=e02fce03a933b5c8fffdf46442a2e8e9ts"
    "1461052882959uunid4tid=23&tck=e02fce03a933b5c8fffdf46442a2e8e9&ts=1461052882959&ver=1ip:106.39.88.82hostname:tf41dg.prod.mediav.comagent:Mozilla/5.0 (Macintosh; Intel Mac OS X 10_11_4)"
    "AppleWebKit/537.36 (KHTML, like Gecko) Chrome/49.0.2623.110 Safari/537.36lang:en,zh-CN;q=0.8,zh;q=0.6,zh-TW;q=0.4referer:http://ckmap.mediav.com/b?type=10proxy:106.39.88.82, 202.79.203.111_mvctn191270=_mvsrc=118216_523698_1043440&_mvcam=191270_1241961_11970810_58740969_0&osr=oqdT0qhmgzj0&time=1449650466&rdom=anonymous; _mvctn165564=_mvsrc=118478_524001_1044112&_mvcam=165564_1224948_11775153_58841999_0&osr=Zg4c0cS49V00&time=1451040575&rdom=anonymous; v=)he((]^XdqB2fOE=[AjT; _jzqa=1.4336274511317232000.1447337529.1447337529.1461045516.2; _jzqc=1; _jzqckmp=1; ckmts=PUPtXAzi,P6PtXAzi,RGPtXAzi,R6PtXAzi,U6PtXAzi,JGPtXAzi,JrPtXAzi,J6PtXAzi,bUPtXAziver10:0|0:0|0:0|0:10p!"
    "version1.01";
  string a;
  struct timeval start, end;
  gettimeofday(&start, NULL);
  for (int i = 0; i < LOOP_COUNT; ++i)
  {
    a = Base64Encode(s);
  }
  std::cout << "base64encoded: " << a << std::endl;
  gettimeofday(&end, NULL);
  std::cout << "Base64Encode timeuse: " << 1000000*(end.tv_sec - start.tv_sec) + (end.tv_usec - start.tv_usec) << std::endl;

  string b;
  gettimeofday(&start, NULL);
  for (int i = 0; i < LOOP_COUNT; ++i)
  {
    b = Base64Encode1(s);
  }
  std::cout << "base64encoded: " << b << std::endl;
  gettimeofday(&end, NULL);
  std::cout << " Base64Encode1 timeuse: " << 1000000*(end.tv_sec - start.tv_sec) + (end.tv_usec - start.tv_usec) << std::endl;

  string e;
  gettimeofday(&start, NULL);
  for (int i = 0; i < LOOP_COUNT; ++i)
  {
    e = Base64Encode2(s);
  }
  std::cout << "base64encoded: " << e << std::endl;
  gettimeofday(&end, NULL);
  std::cout << " Base64Encode2 timeuse: " << 1000000*(end.tv_sec - start.tv_sec) + (end.tv_usec - start.tv_usec) << std::endl;

  string f;
  gettimeofday(&start, NULL);
  for (int i = 0; i < LOOP_COUNT; ++i)
  {
    f = Base64Encode3(s);
  }
  std::cout << "base64encoded: " << e << std::endl;
  gettimeofday(&end, NULL);
  std::cout << " Base64Encode3 timeuse: " << 1000000*(end.tv_sec - start.tv_sec) + (end.tv_usec - start.tv_usec) << std::endl;

  string c;
  gettimeofday(&start, NULL);
  for (int i = 0; i < LOOP_COUNT; ++i)
  {
    Base64EncodeModp(s, &c);
  }
  std::cout << "base64encoded: " << c << std::endl;
  gettimeofday(&end, NULL);
  std::cout << " Base64Encode1 timeuse: " << 1000000*(end.tv_sec - start.tv_sec) + (end.tv_usec - start.tv_usec) << std::endl;
  return 0;
}
<|endoftext|>


<commit_before>/*
 * 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_ros/map_builder_bridge.h"

#include "cartographer/common/make_unique.h"
#include "cartographer/io/color.h"
#include "cartographer/io/proto_stream.h"
#include "cartographer/mapping/pose_graph.h"
#include "cartographer_ros/msg_conversion.h"

namespace cartographer_ros {

namespace {

constexpr double kTrajectoryLineStripMarkerScale = 0.07;
constexpr double kConstraintMarkerScale = 0.025;

::std_msgs::ColorRGBA ToMessage(const cartographer::io::FloatColor& color) {
  ::std_msgs::ColorRGBA result;
  result.r = color[0];
  result.g = color[1];
  result.b = color[2];
  result.a = 1.f;
  return result;
}

visualization_msgs::Marker CreateTrajectoryMarker(const int trajectory_id,
                                                  const std::string& frame_id) {
  visualization_msgs::Marker marker;
  marker.ns = "Trajectory " + std::to_string(trajectory_id);
  marker.id = 0;
  marker.type = visualization_msgs::Marker::LINE_STRIP;
  marker.header.stamp = ::ros::Time::now();
  marker.header.frame_id = frame_id;
  marker.color = ToMessage(cartographer::io::GetColor(trajectory_id));
  marker.scale.x = kTrajectoryLineStripMarkerScale;
  marker.pose.orientation.w = 1.;
  marker.pose.position.z = 0.05;
  return marker;
}

void PushAndResetLineMarker(visualization_msgs::Marker* marker,
                            std::vector<visualization_msgs::Marker>* markers) {
  if (marker->points.size() > 1) {
    markers->push_back(*marker);
    ++marker->id;
  }
  marker->points.clear();
}

}  // namespace

MapBuilderBridge::MapBuilderBridge(
    const NodeOptions& node_options,
    std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
    tf2_ros::Buffer* const tf_buffer)
    : node_options_(node_options),
      map_builder_(std::move(map_builder)),
      tf_buffer_(tf_buffer) {}

void MapBuilderBridge::LoadMap(const std::string& map_filename) {
  LOG(INFO) << "Loading map '" << map_filename << "'...";
  cartographer::io::ProtoStreamReader stream(map_filename);
  map_builder_->LoadMap(&stream);
}

int MapBuilderBridge::AddTrajectory(
    const std::unordered_set<std::string>& expected_sensor_ids,
    const TrajectoryOptions& trajectory_options) {
  const int trajectory_id = map_builder_->AddTrajectoryBuilder(
      expected_sensor_ids, trajectory_options.trajectory_builder_options,
      ::std::bind(&MapBuilderBridge::OnLocalSlamResult, this,
                  ::std::placeholders::_1, ::std::placeholders::_2,
                  ::std::placeholders::_3, ::std::placeholders::_4,
                  ::std::placeholders::_5));
  LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'.";

  // Make sure there is no trajectory with 'trajectory_id' yet.
  CHECK_EQ(sensor_bridges_.count(trajectory_id), 0);
  sensor_bridges_[trajectory_id] =
      cartographer::common::make_unique<SensorBridge>(
          trajectory_options.num_subdivisions_per_laser_scan,
          trajectory_options.tracking_frame,
          node_options_.lookup_transform_timeout_sec, tf_buffer_,
          map_builder_->GetTrajectoryBuilder(trajectory_id));
  auto emplace_result =
      trajectory_options_.emplace(trajectory_id, trajectory_options);
  CHECK(emplace_result.second == true);
  return trajectory_id;
}

void MapBuilderBridge::FinishTrajectory(const int trajectory_id) {
  LOG(INFO) << "Finishing trajectory with ID '" << trajectory_id << "'...";

  // Make sure there is a trajectory with 'trajectory_id'.
  CHECK_EQ(sensor_bridges_.count(trajectory_id), 1);
  map_builder_->FinishTrajectory(trajectory_id);
  sensor_bridges_.erase(trajectory_id);
}

void MapBuilderBridge::RunFinalOptimization() {
  LOG(INFO) << "Running final trajectory optimization...";
  map_builder_->pose_graph()->RunFinalOptimization();
}

void MapBuilderBridge::SerializeState(const std::string& filename) {
  cartographer::io::ProtoStreamWriter writer(filename);
  map_builder_->SerializeState(&writer);
  CHECK(writer.Close()) << "Could not write state.";
}

bool MapBuilderBridge::HandleSubmapQuery(
    cartographer_ros_msgs::SubmapQuery::Request& request,
    cartographer_ros_msgs::SubmapQuery::Response& response) {
  cartographer::mapping::proto::SubmapQuery::Response response_proto;
  cartographer::mapping::SubmapId submap_id{request.trajectory_id,
                                            request.submap_index};
  const std::string error =
      map_builder_->SubmapToProto(submap_id, &response_proto);
  if (!error.empty()) {
    LOG(ERROR) << error;
    return false;
  }

  CHECK(response_proto.textures_size() > 0)
      << "empty textures given for submap: " << submap_id;

  response.submap_version = response_proto.submap_version();
  for (const auto& texture_proto : response_proto.textures()) {
    response.textures.emplace_back();
    auto& texture = response.textures.back();
    texture.cells.insert(texture.cells.begin(), texture_proto.cells().begin(),
                         texture_proto.cells().end());
    texture.width = texture_proto.width();
    texture.height = texture_proto.height();
    texture.resolution = texture_proto.resolution();
    texture.slice_pose = ToGeometryMsgPose(
        cartographer::transform::ToRigid3(texture_proto.slice_pose()));
  }
  return true;
}

cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {
  cartographer_ros_msgs::SubmapList submap_list;
  submap_list.header.stamp = ::ros::Time::now();
  submap_list.header.frame_id = node_options_.map_frame;
  for (const auto& submap_id_pose :
       map_builder_->pose_graph()->GetAllSubmapPoses()) {
    cartographer_ros_msgs::SubmapEntry submap_entry;
    submap_entry.trajectory_id = submap_id_pose.id.trajectory_id;
    submap_entry.submap_index = submap_id_pose.id.submap_index;
    submap_entry.submap_version = submap_id_pose.data.version;
    submap_entry.pose = ToGeometryMsgPose(submap_id_pose.data.pose);
    submap_list.submap.push_back(submap_entry);
  }
  return submap_list;
}

std::unordered_map<int, MapBuilderBridge::TrajectoryState>
MapBuilderBridge::GetTrajectoryStates() {
  std::unordered_map<int, TrajectoryState> trajectory_states;
  for (const auto& entry : sensor_bridges_) {
    const int trajectory_id = entry.first;
    const SensorBridge& sensor_bridge = *entry.second;

    std::shared_ptr<const TrajectoryState::LocalSlamData> local_slam_data;
    {
      cartographer::common::MutexLocker lock(&mutex_);
      if (trajectory_state_data_.count(trajectory_id) == 0) {
        continue;
      }
      local_slam_data = trajectory_state_data_.at(trajectory_id);
    }

    // Make sure there is a trajectory with 'trajectory_id'.
    CHECK_EQ(trajectory_options_.count(trajectory_id), 1);
    trajectory_states[trajectory_id] = {
        local_slam_data,
        map_builder_->pose_graph()->GetLocalToGlobalTransform(trajectory_id),
        sensor_bridge.tf_bridge().LookupToTracking(
            local_slam_data->time,
            trajectory_options_[trajectory_id].published_frame),
        trajectory_options_[trajectory_id]};
  }
  return trajectory_states;
}

visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() {
  visualization_msgs::MarkerArray trajectory_node_list;
  const auto node_poses = map_builder_->pose_graph()->GetTrajectoryNodePoses();
  for (const int trajectory_id : node_poses.trajectory_ids()) {
    visualization_msgs::Marker marker =
        CreateTrajectoryMarker(trajectory_id, node_options_.map_frame);

    for (const auto& node_id_data : node_poses.trajectory(trajectory_id)) {
      if (node_id_data.data.has_constant_data) {
        PushAndResetLineMarker(&marker, &trajectory_node_list.markers);
        continue;
      }
      const ::geometry_msgs::Point node_point =
          ToGeometryMsgPoint(node_id_data.data.global_pose.translation());
      marker.points.push_back(node_point);
      // Work around the 16384 point limit in RViz by splitting the
      // trajectory into multiple markers.
      if (marker.points.size() == 16384) {
        PushAndResetLineMarker(&marker, &trajectory_node_list.markers);
        // Push back the last point, so the two markers appear connected.
        marker.points.push_back(node_point);
      }
    }
    PushAndResetLineMarker(&marker, &trajectory_node_list.markers);
  }
  return trajectory_node_list;
}

visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() {
  visualization_msgs::MarkerArray constraint_list;
  int marker_id = 0;
  visualization_msgs::Marker constraint_intra_marker;
  constraint_intra_marker.id = marker_id++;
  constraint_intra_marker.ns = "Intra constraints";
  constraint_intra_marker.type = visualization_msgs::Marker::LINE_LIST;
  constraint_intra_marker.header.stamp = ros::Time::now();
  constraint_intra_marker.header.frame_id = node_options_.map_frame;
  constraint_intra_marker.scale.x = kConstraintMarkerScale;
  constraint_intra_marker.pose.orientation.w = 1.0;

  visualization_msgs::Marker residual_intra_marker = constraint_intra_marker;
  residual_intra_marker.id = marker_id++;
  residual_intra_marker.ns = "Intra residuals";
  // This and other markers which are less numerous are set to be slightly
  // above the intra constraints marker in order to ensure that they are
  // visible.
  residual_intra_marker.pose.position.z = 0.1;

  visualization_msgs::Marker constraint_inter_marker = constraint_intra_marker;
  constraint_inter_marker.id = marker_id++;
  constraint_inter_marker.ns = "Inter constraints";
  constraint_inter_marker.pose.position.z = 0.1;

  visualization_msgs::Marker residual_inter_marker = constraint_intra_marker;
  residual_inter_marker.id = marker_id++;
  residual_inter_marker.ns = "Inter residuals";
  residual_inter_marker.pose.position.z = 0.1;

  const auto trajectory_nodes =
      map_builder_->pose_graph()->GetTrajectoryNodes();
  const auto submap_data = map_builder_->pose_graph()->GetAllSubmapData();
  const auto constraints = map_builder_->pose_graph()->constraints();

  for (const auto& constraint : constraints) {
    visualization_msgs::Marker *constraint_marker, *residual_marker;
    std_msgs::ColorRGBA color_constraint, color_residual;
    if (constraint.tag ==
        cartographer::mapping::PoseGraph::Constraint::INTRA_SUBMAP) {
      constraint_marker = &constraint_intra_marker;
      residual_marker = &residual_intra_marker;
      // Color mapping for submaps of various trajectories - add trajectory id
      // to ensure different starting colors. Also add a fixed offset of 25
      // to avoid having identical colors as trajectories.
      color_constraint = ToMessage(
          cartographer::io::GetColor(constraint.submap_id.submap_index +
                                     constraint.submap_id.trajectory_id + 25));
      color_residual.a = 1.0;
      color_residual.r = 1.0;
    } else {
      constraint_marker = &constraint_inter_marker;
      residual_marker = &residual_inter_marker;
      // Bright yellow
      color_constraint.a = 1.0;
      color_constraint.r = color_constraint.g = 1.0;
      // Bright cyan
      color_residual.a = 1.0;
      color_residual.b = color_residual.g = 1.0;
    }

    for (int i = 0; i < 2; ++i) {
      constraint_marker->colors.push_back(color_constraint);
      residual_marker->colors.push_back(color_residual);
    }

    const auto submap_it = submap_data.find(constraint.submap_id);
    if (submap_it == submap_data.end()) {
      continue;
    }
    const auto& submap_pose = submap_it->data.pose;
    const auto node_it = trajectory_nodes.find(constraint.node_id);
    if (node_it == trajectory_nodes.end()) {
      continue;
    }
    const auto& trajectory_node_pose = node_it->data.global_pose;
    const cartographer::transform::Rigid3d constraint_pose =
        submap_pose * constraint.pose.zbar_ij;

    constraint_marker->points.push_back(
        ToGeometryMsgPoint(submap_pose.translation()));
    constraint_marker->points.push_back(
        ToGeometryMsgPoint(constraint_pose.translation()));

    residual_marker->points.push_back(
        ToGeometryMsgPoint(constraint_pose.translation()));
    residual_marker->points.push_back(
        ToGeometryMsgPoint(trajectory_node_pose.translation()));
  }

  constraint_list.markers.push_back(constraint_intra_marker);
  constraint_list.markers.push_back(residual_intra_marker);
  constraint_list.markers.push_back(constraint_inter_marker);
  constraint_list.markers.push_back(residual_inter_marker);
  return constraint_list;
}

SensorBridge* MapBuilderBridge::sensor_bridge(const int trajectory_id) {
  return sensor_bridges_.at(trajectory_id).get();
}

void MapBuilderBridge::OnLocalSlamResult(
    const int trajectory_id, const ::cartographer::common::Time time,
    const ::cartographer::transform::Rigid3d local_pose,
    ::cartographer::sensor::RangeData range_data_in_local,
    const std::unique_ptr<const ::cartographer::mapping::NodeId>) {
  std::shared_ptr<const TrajectoryState::LocalSlamData> local_slam_data =
      std::make_shared<TrajectoryState::LocalSlamData>(
          TrajectoryState::LocalSlamData{time, local_pose,
                                         std::move(range_data_in_local)});
  cartographer::common::MutexLocker lock(&mutex_);
  trajectory_state_data_[trajectory_id] = std::move(local_slam_data);
}

}  // namespace cartographer_ros
<commit_msg>Use GetTrajectoryNodePoses and GetAllSubmapPoses in GetConstraintList (#651)<commit_after>/*
 * 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_ros/map_builder_bridge.h"

#include "cartographer/common/make_unique.h"
#include "cartographer/io/color.h"
#include "cartographer/io/proto_stream.h"
#include "cartographer/mapping/pose_graph.h"
#include "cartographer_ros/msg_conversion.h"

namespace cartographer_ros {

namespace {

constexpr double kTrajectoryLineStripMarkerScale = 0.07;
constexpr double kConstraintMarkerScale = 0.025;

::std_msgs::ColorRGBA ToMessage(const cartographer::io::FloatColor& color) {
  ::std_msgs::ColorRGBA result;
  result.r = color[0];
  result.g = color[1];
  result.b = color[2];
  result.a = 1.f;
  return result;
}

visualization_msgs::Marker CreateTrajectoryMarker(const int trajectory_id,
                                                  const std::string& frame_id) {
  visualization_msgs::Marker marker;
  marker.ns = "Trajectory " + std::to_string(trajectory_id);
  marker.id = 0;
  marker.type = visualization_msgs::Marker::LINE_STRIP;
  marker.header.stamp = ::ros::Time::now();
  marker.header.frame_id = frame_id;
  marker.color = ToMessage(cartographer::io::GetColor(trajectory_id));
  marker.scale.x = kTrajectoryLineStripMarkerScale;
  marker.pose.orientation.w = 1.;
  marker.pose.position.z = 0.05;
  return marker;
}

void PushAndResetLineMarker(visualization_msgs::Marker* marker,
                            std::vector<visualization_msgs::Marker>* markers) {
  if (marker->points.size() > 1) {
    markers->push_back(*marker);
    ++marker->id;
  }
  marker->points.clear();
}

}  // namespace

MapBuilderBridge::MapBuilderBridge(
    const NodeOptions& node_options,
    std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
    tf2_ros::Buffer* const tf_buffer)
    : node_options_(node_options),
      map_builder_(std::move(map_builder)),
      tf_buffer_(tf_buffer) {}

void MapBuilderBridge::LoadMap(const std::string& map_filename) {
  LOG(INFO) << "Loading map '" << map_filename << "'...";
  cartographer::io::ProtoStreamReader stream(map_filename);
  map_builder_->LoadMap(&stream);
}

int MapBuilderBridge::AddTrajectory(
    const std::unordered_set<std::string>& expected_sensor_ids,
    const TrajectoryOptions& trajectory_options) {
  const int trajectory_id = map_builder_->AddTrajectoryBuilder(
      expected_sensor_ids, trajectory_options.trajectory_builder_options,
      ::std::bind(&MapBuilderBridge::OnLocalSlamResult, this,
                  ::std::placeholders::_1, ::std::placeholders::_2,
                  ::std::placeholders::_3, ::std::placeholders::_4,
                  ::std::placeholders::_5));
  LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'.";

  // Make sure there is no trajectory with 'trajectory_id' yet.
  CHECK_EQ(sensor_bridges_.count(trajectory_id), 0);
  sensor_bridges_[trajectory_id] =
      cartographer::common::make_unique<SensorBridge>(
          trajectory_options.num_subdivisions_per_laser_scan,
          trajectory_options.tracking_frame,
          node_options_.lookup_transform_timeout_sec, tf_buffer_,
          map_builder_->GetTrajectoryBuilder(trajectory_id));
  auto emplace_result =
      trajectory_options_.emplace(trajectory_id, trajectory_options);
  CHECK(emplace_result.second == true);
  return trajectory_id;
}

void MapBuilderBridge::FinishTrajectory(const int trajectory_id) {
  LOG(INFO) << "Finishing trajectory with ID '" << trajectory_id << "'...";

  // Make sure there is a trajectory with 'trajectory_id'.
  CHECK_EQ(sensor_bridges_.count(trajectory_id), 1);
  map_builder_->FinishTrajectory(trajectory_id);
  sensor_bridges_.erase(trajectory_id);
}

void MapBuilderBridge::RunFinalOptimization() {
  LOG(INFO) << "Running final trajectory optimization...";
  map_builder_->pose_graph()->RunFinalOptimization();
}

void MapBuilderBridge::SerializeState(const std::string& filename) {
  cartographer::io::ProtoStreamWriter writer(filename);
  map_builder_->SerializeState(&writer);
  CHECK(writer.Close()) << "Could not write state.";
}

bool MapBuilderBridge::HandleSubmapQuery(
    cartographer_ros_msgs::SubmapQuery::Request& request,
    cartographer_ros_msgs::SubmapQuery::Response& response) {
  cartographer::mapping::proto::SubmapQuery::Response response_proto;
  cartographer::mapping::SubmapId submap_id{request.trajectory_id,
                                            request.submap_index};
  const std::string error =
      map_builder_->SubmapToProto(submap_id, &response_proto);
  if (!error.empty()) {
    LOG(ERROR) << error;
    return false;
  }

  CHECK(response_proto.textures_size() > 0)
      << "empty textures given for submap: " << submap_id;

  response.submap_version = response_proto.submap_version();
  for (const auto& texture_proto : response_proto.textures()) {
    response.textures.emplace_back();
    auto& texture = response.textures.back();
    texture.cells.insert(texture.cells.begin(), texture_proto.cells().begin(),
                         texture_proto.cells().end());
    texture.width = texture_proto.width();
    texture.height = texture_proto.height();
    texture.resolution = texture_proto.resolution();
    texture.slice_pose = ToGeometryMsgPose(
        cartographer::transform::ToRigid3(texture_proto.slice_pose()));
  }
  return true;
}

cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {
  cartographer_ros_msgs::SubmapList submap_list;
  submap_list.header.stamp = ::ros::Time::now();
  submap_list.header.frame_id = node_options_.map_frame;
  for (const auto& submap_id_pose :
       map_builder_->pose_graph()->GetAllSubmapPoses()) {
    cartographer_ros_msgs::SubmapEntry submap_entry;
    submap_entry.trajectory_id = submap_id_pose.id.trajectory_id;
    submap_entry.submap_index = submap_id_pose.id.submap_index;
    submap_entry.submap_version = submap_id_pose.data.version;
    submap_entry.pose = ToGeometryMsgPose(submap_id_pose.data.pose);
    submap_list.submap.push_back(submap_entry);
  }
  return submap_list;
}

std::unordered_map<int, MapBuilderBridge::TrajectoryState>
MapBuilderBridge::GetTrajectoryStates() {
  std::unordered_map<int, TrajectoryState> trajectory_states;
  for (const auto& entry : sensor_bridges_) {
    const int trajectory_id = entry.first;
    const SensorBridge& sensor_bridge = *entry.second;

    std::shared_ptr<const TrajectoryState::LocalSlamData> local_slam_data;
    {
      cartographer::common::MutexLocker lock(&mutex_);
      if (trajectory_state_data_.count(trajectory_id) == 0) {
        continue;
      }
      local_slam_data = trajectory_state_data_.at(trajectory_id);
    }

    // Make sure there is a trajectory with 'trajectory_id'.
    CHECK_EQ(trajectory_options_.count(trajectory_id), 1);
    trajectory_states[trajectory_id] = {
        local_slam_data,
        map_builder_->pose_graph()->GetLocalToGlobalTransform(trajectory_id),
        sensor_bridge.tf_bridge().LookupToTracking(
            local_slam_data->time,
            trajectory_options_[trajectory_id].published_frame),
        trajectory_options_[trajectory_id]};
  }
  return trajectory_states;
}

visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() {
  visualization_msgs::MarkerArray trajectory_node_list;
  const auto node_poses = map_builder_->pose_graph()->GetTrajectoryNodePoses();
  for (const int trajectory_id : node_poses.trajectory_ids()) {
    visualization_msgs::Marker marker =
        CreateTrajectoryMarker(trajectory_id, node_options_.map_frame);

    for (const auto& node_id_data : node_poses.trajectory(trajectory_id)) {
      if (node_id_data.data.has_constant_data) {
        PushAndResetLineMarker(&marker, &trajectory_node_list.markers);
        continue;
      }
      const ::geometry_msgs::Point node_point =
          ToGeometryMsgPoint(node_id_data.data.global_pose.translation());
      marker.points.push_back(node_point);
      // Work around the 16384 point limit in RViz by splitting the
      // trajectory into multiple markers.
      if (marker.points.size() == 16384) {
        PushAndResetLineMarker(&marker, &trajectory_node_list.markers);
        // Push back the last point, so the two markers appear connected.
        marker.points.push_back(node_point);
      }
    }
    PushAndResetLineMarker(&marker, &trajectory_node_list.markers);
  }
  return trajectory_node_list;
}

visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() {
  visualization_msgs::MarkerArray constraint_list;
  int marker_id = 0;
  visualization_msgs::Marker constraint_intra_marker;
  constraint_intra_marker.id = marker_id++;
  constraint_intra_marker.ns = "Intra constraints";
  constraint_intra_marker.type = visualization_msgs::Marker::LINE_LIST;
  constraint_intra_marker.header.stamp = ros::Time::now();
  constraint_intra_marker.header.frame_id = node_options_.map_frame;
  constraint_intra_marker.scale.x = kConstraintMarkerScale;
  constraint_intra_marker.pose.orientation.w = 1.0;

  visualization_msgs::Marker residual_intra_marker = constraint_intra_marker;
  residual_intra_marker.id = marker_id++;
  residual_intra_marker.ns = "Intra residuals";
  // This and other markers which are less numerous are set to be slightly
  // above the intra constraints marker in order to ensure that they are
  // visible.
  residual_intra_marker.pose.position.z = 0.1;

  visualization_msgs::Marker constraint_inter_marker = constraint_intra_marker;
  constraint_inter_marker.id = marker_id++;
  constraint_inter_marker.ns = "Inter constraints";
  constraint_inter_marker.pose.position.z = 0.1;

  visualization_msgs::Marker residual_inter_marker = constraint_intra_marker;
  residual_inter_marker.id = marker_id++;
  residual_inter_marker.ns = "Inter residuals";
  residual_inter_marker.pose.position.z = 0.1;

  const auto trajectory_node_poses =
      map_builder_->pose_graph()->GetTrajectoryNodePoses();
  const auto submap_poses = map_builder_->pose_graph()->GetAllSubmapPoses();
  const auto constraints = map_builder_->pose_graph()->constraints();

  for (const auto& constraint : constraints) {
    visualization_msgs::Marker *constraint_marker, *residual_marker;
    std_msgs::ColorRGBA color_constraint, color_residual;
    if (constraint.tag ==
        cartographer::mapping::PoseGraph::Constraint::INTRA_SUBMAP) {
      constraint_marker = &constraint_intra_marker;
      residual_marker = &residual_intra_marker;
      // Color mapping for submaps of various trajectories - add trajectory id
      // to ensure different starting colors. Also add a fixed offset of 25
      // to avoid having identical colors as trajectories.
      color_constraint = ToMessage(
          cartographer::io::GetColor(constraint.submap_id.submap_index +
                                     constraint.submap_id.trajectory_id + 25));
      color_residual.a = 1.0;
      color_residual.r = 1.0;
    } else {
      constraint_marker = &constraint_inter_marker;
      residual_marker = &residual_inter_marker;
      // Bright yellow
      color_constraint.a = 1.0;
      color_constraint.r = color_constraint.g = 1.0;
      // Bright cyan
      color_residual.a = 1.0;
      color_residual.b = color_residual.g = 1.0;
    }

    for (int i = 0; i < 2; ++i) {
      constraint_marker->colors.push_back(color_constraint);
      residual_marker->colors.push_back(color_residual);
    }

    const auto submap_it = submap_poses.find(constraint.submap_id);
    if (submap_it == submap_poses.end()) {
      continue;
    }
    const auto& submap_pose = submap_it->data.pose;
    const auto node_it = trajectory_node_poses.find(constraint.node_id);
    if (node_it == trajectory_node_poses.end()) {
      continue;
    }
    const auto& trajectory_node_pose = node_it->data.global_pose;
    const cartographer::transform::Rigid3d constraint_pose =
        submap_pose * constraint.pose.zbar_ij;

    constraint_marker->points.push_back(
        ToGeometryMsgPoint(submap_pose.translation()));
    constraint_marker->points.push_back(
        ToGeometryMsgPoint(constraint_pose.translation()));

    residual_marker->points.push_back(
        ToGeometryMsgPoint(constraint_pose.translation()));
    residual_marker->points.push_back(
        ToGeometryMsgPoint(trajectory_node_pose.translation()));
  }

  constraint_list.markers.push_back(constraint_intra_marker);
  constraint_list.markers.push_back(residual_intra_marker);
  constraint_list.markers.push_back(constraint_inter_marker);
  constraint_list.markers.push_back(residual_inter_marker);
  return constraint_list;
}

SensorBridge* MapBuilderBridge::sensor_bridge(const int trajectory_id) {
  return sensor_bridges_.at(trajectory_id).get();
}

void MapBuilderBridge::OnLocalSlamResult(
    const int trajectory_id, const ::cartographer::common::Time time,
    const ::cartographer::transform::Rigid3d local_pose,
    ::cartographer::sensor::RangeData range_data_in_local,
    const std::unique_ptr<const ::cartographer::mapping::NodeId>) {
  std::shared_ptr<const TrajectoryState::LocalSlamData> local_slam_data =
      std::make_shared<TrajectoryState::LocalSlamData>(
          TrajectoryState::LocalSlamData{time, local_pose,
                                         std::move(range_data_in_local)});
  cartographer::common::MutexLocker lock(&mutex_);
  trajectory_state_data_[trajectory_id] = std::move(local_slam_data);
}

}  // namespace cartographer_ros
<|endoftext|>


<commit_before>/*
 * This file is part of Maliit Plugins
 *
 * Copyright (C) 2011 Nokia Corporation and/or its subsidiary(-ies). All rights reserved.
 *
 * Contact: Mohammad Anwari <Mohammad.Anwari@nokia.com>
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * Redistributions of source code must retain the above copyright notice, this list
 * of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice, this list
 * of conditions and the following disclaimer in the documentation and/or other materials
 * provided with the distribution.
 * Neither the name of Nokia Corporation nor the names of its contributors may be
 * used to endorse or promote products derived from this software without specific
 * prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL
 * THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include "view/renderer.h"
#include "view/glass.h"
#include "logic/layoutupdater.h"

#include "models/keyarea.h"
#include "models/key.h"
#include "models/keylabel.h"
#include "models/layout.h"

#include <QtGui>

namespace {
MaliitKeyboard::Key createKey(const QPixmap &pm,
                              const MaliitKeyboard::SharedFont &f,
                              const QRect &kr,
                              const QRect &lr,
                              const QByteArray &t,
                              const QColor &c,
                              MaliitKeyboard::Key::Action a = MaliitKeyboard::Key::ActionInsert)
{
    MaliitKeyboard::KeyLabel l;
    l.setText(t);
    l.setColor(c);
    l.setFont(f);

    MaliitKeyboard::Key k;
    k.setRect(kr);
    k.setBackground(pm);
    k.setLabel(l);
    k.setAction(a);

    return k;
}

MaliitKeyboard::KeyArea createPrimaryKeyArea()
{
    typedef QByteArray QBA;

    QPixmap pm(8, 8);
    pm.fill(Qt::lightGray);

    MaliitKeyboard::SharedFont font(new QFont);
    font->setBold(true);
    font->setPointSize(16);

    MaliitKeyboard::KeyArea ka;
    ka.rect = QRectF(0, 554, 480, 300);
    ka.keys.append(createKey(pm, font, QRect(10, 10, 40, 60),
                             QRect(5, 5, 20, 40), QBA("Q"), Qt::darkBlue));
    ka.keys.append(createKey(pm, font, QRect(60, 10, 80, 120),
                             QRect(5, 5, 70, 40), QBA("W"), Qt::darkMagenta));
    ka.keys.append(createKey(pm, font, QRect(10, 80, 40, 50),
                             QRect(5, 5, 20, 40), QBA("A"), Qt::black));
    ka.keys.append(createKey(pm, font, QRect(10, 140, 130, 60),
                             QRect(5, 5, 120, 40), QBA("shift"), Qt::darkCyan,
                             MaliitKeyboard::Key::ActionShift));
    ka.keys.append(createKey(pm, font, QRect(160, 10, 120, 120),
                             QRect(5, 5, 120, 40), QBA("switch"), Qt::darkCyan,
                             MaliitKeyboard::Key::ActionSwitch));

    return ka;
}

MaliitKeyboard::KeyArea createSecondaryKeyArea()
{
    typedef QByteArray QBA;

    QPixmap pm(8, 8);
    pm.fill(Qt::lightGray);

    MaliitKeyboard::SharedFont font(new QFont);
    font->setBold(true);
    font->setPointSize(16);

    MaliitKeyboard::KeyArea ka;
    ka.rect = QRectF(0, 0, 480, 100);
    ka.keys.append(createKey(pm, font, QRect(10, 10, 40, 60),
                             QRect(5, 5, 20, 40), QBA("T"), Qt::darkBlue));
    ka.keys.append(createKey(pm, font, QRect(60, 10, 80, 80),
                             QRect(5, 5, 20, 40), QBA("O"), Qt::darkMagenta));

    return ka;
}

}

int main(int argc,
         char ** argv)
{
    QApplication app(argc, argv);

    QWidget *window = new QWidget;
    window->resize(480, 854);
    window->showFullScreen();

    MaliitKeyboard::SharedLayout l0(new MaliitKeyboard::Layout);
    l0->setCenterPanel(createPrimaryKeyArea());

    MaliitKeyboard::SharedLayout l1(new MaliitKeyboard::Layout);
    l1->setCenterPanel(createSecondaryKeyArea());

    MaliitKeyboard::Renderer renderer;
    renderer.setWindow(window);
    renderer.addLayout(l0);
    renderer.addLayout(l1);
    renderer.show();

    MaliitKeyboard::Glass glass;
    glass.setWindow(renderer.viewport());
    glass.addLayout(l0);
    glass.addLayout(l1);

    // One layout updater can only manage one layout. If more layouts need to
    // be managed, then more layout updaters are required.
    MaliitKeyboard::LayoutUpdater updater;
    updater.init();
    updater.setLayout(l0);

    QObject::connect(&glass,   SIGNAL(keyPressed(Key, SharedLayout)),
                     &updater, SLOT(onKeyPressed(Key, SharedLayout)));

    QObject::connect(&glass,   SIGNAL(keyReleased(Key, SharedLayout)),
                     &updater, SLOT(onKeyReleased(Key, SharedLayout)));

    QObject::connect(&updater,  SIGNAL(layoutChanged(SharedLayout)),
                     &renderer, SLOT(onLayoutChanged(SharedLayout)));

    QObject::connect(&updater,  SIGNAL(keysChanged(SharedLayout)),
                     &renderer, SLOT(onKeysChanged(SharedLayout)));

    return app.exec();
}
<commit_msg>Remove unused KeyLabel rectangle parameters from maliit-keyboard-viewer<commit_after>/*
 * This file is part of Maliit Plugins
 *
 * Copyright (C) 2011 Nokia Corporation and/or its subsidiary(-ies). All rights reserved.
 *
 * Contact: Mohammad Anwari <Mohammad.Anwari@nokia.com>
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * Redistributions of source code must retain the above copyright notice, this list
 * of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice, this list
 * of conditions and the following disclaimer in the documentation and/or other materials
 * provided with the distribution.
 * Neither the name of Nokia Corporation nor the names of its contributors may be
 * used to endorse or promote products derived from this software without specific
 * prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL
 * THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include "view/renderer.h"
#include "view/glass.h"
#include "logic/layoutupdater.h"

#include "models/keyarea.h"
#include "models/key.h"
#include "models/keylabel.h"
#include "models/layout.h"

#include <QtGui>

namespace {
MaliitKeyboard::Key createKey(const QPixmap &pm,
                              const MaliitKeyboard::SharedFont &f,
                              const QRect &kr,
                              const QByteArray &t,
                              const QColor &c,
                              MaliitKeyboard::Key::Action a = MaliitKeyboard::Key::ActionInsert)
{
    MaliitKeyboard::KeyLabel l;
    l.setText(t);
    l.setColor(c);
    l.setFont(f);

    MaliitKeyboard::Key k;
    k.setRect(kr);
    k.setBackground(pm);
    k.setLabel(l);
    k.setAction(a);

    return k;
}

MaliitKeyboard::KeyArea createPrimaryKeyArea()
{
    typedef QByteArray QBA;

    QPixmap pm(8, 8);
    pm.fill(Qt::lightGray);

    MaliitKeyboard::SharedFont font(new QFont);
    font->setBold(true);
    font->setPointSize(16);

    MaliitKeyboard::KeyArea ka;
    ka.rect = QRectF(0, 554, 480, 300);
    ka.keys.append(createKey(pm, font, QRect(10, 10, 40, 60), QBA("Q"), Qt::darkBlue));
    ka.keys.append(createKey(pm, font, QRect(60, 10, 80, 120), QBA("W"), Qt::darkMagenta));
    ka.keys.append(createKey(pm, font, QRect(10, 80, 40, 50), QBA("A"), Qt::black));
    ka.keys.append(createKey(pm, font, QRect(10, 140, 130, 60), QBA("shift"), Qt::darkCyan,
                             MaliitKeyboard::Key::ActionShift));
    ka.keys.append(createKey(pm, font, QRect(160, 10, 120, 120), QBA("switch"), Qt::darkCyan,
                             MaliitKeyboard::Key::ActionSwitch));

    return ka;
}

MaliitKeyboard::KeyArea createSecondaryKeyArea()
{
    typedef QByteArray QBA;

    QPixmap pm(8, 8);
    pm.fill(Qt::lightGray);

    MaliitKeyboard::SharedFont font(new QFont);
    font->setBold(true);
    font->setPointSize(16);

    MaliitKeyboard::KeyArea ka;
    ka.rect = QRectF(0, 0, 480, 100);
    ka.keys.append(createKey(pm, font, QRect(10, 10, 40, 60), QBA("T"), Qt::darkBlue));
    ka.keys.append(createKey(pm, font, QRect(60, 10, 80, 80), QBA("O"), Qt::darkMagenta));

    return ka;
}

}

int main(int argc,
         char ** argv)
{
    QApplication app(argc, argv);

    QWidget *window = new QWidget;
    window->resize(480, 854);
    window->showFullScreen();

    MaliitKeyboard::SharedLayout l0(new MaliitKeyboard::Layout);
    l0->setCenterPanel(createPrimaryKeyArea());

    MaliitKeyboard::SharedLayout l1(new MaliitKeyboard::Layout);
    l1->setCenterPanel(createSecondaryKeyArea());

    MaliitKeyboard::Renderer renderer;
    renderer.setWindow(window);
    renderer.addLayout(l0);
    renderer.addLayout(l1);
    renderer.show();

    MaliitKeyboard::Glass glass;
    glass.setWindow(renderer.viewport());
    glass.addLayout(l0);
    glass.addLayout(l1);

    // One layout updater can only manage one layout. If more layouts need to
    // be managed, then more layout updaters are required.
    MaliitKeyboard::LayoutUpdater updater;
    updater.init();
    updater.setLayout(l0);

    QObject::connect(&glass,   SIGNAL(keyPressed(Key, SharedLayout)),
                     &updater, SLOT(onKeyPressed(Key, SharedLayout)));

    QObject::connect(&glass,   SIGNAL(keyReleased(Key, SharedLayout)),
                     &updater, SLOT(onKeyReleased(Key, SharedLayout)));

    QObject::connect(&updater,  SIGNAL(layoutChanged(SharedLayout)),
                     &renderer, SLOT(onLayoutChanged(SharedLayout)));

    QObject::connect(&updater,  SIGNAL(keysChanged(SharedLayout)),
                     &renderer, SLOT(onKeysChanged(SharedLayout)));

    return app.exec();
}
<|endoftext|>


<commit_before>/*
 * Copyright 2014 Open Connectome Project (http://openconnecto.me)
 * Written by Da Zheng (zhengda1936@gmail.com)
 *
 * This file is part of FlashMatrix.
 *
 * 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 <unordered_map>

#include "matrix_worker_thread.h"
#include "sparse_matrix.h"

namespace fm
{

static const int MAX_PENDING_IOS = 32;

class matrix_io_callback: public safs::callback
{
	typedef std::unordered_map<char *, compute_task::ptr> task_map_t;
	task_map_t tasks;
	size_t pending_size;
public:
	matrix_io_callback() {
		pending_size = 0;
	}

	~matrix_io_callback() {
		assert(pending_size == 0);
	}

	size_t get_pending_size() const {
		return pending_size;
	}

	size_t get_pending_ios() const {
		return tasks.size();
	}

	int invoke(safs::io_request *reqs[], int num);
	void add_task(const safs::io_request &req, compute_task::ptr task) {
		tasks.insert(task_map_t::value_type(req.get_buf(), task));
		pending_size += req.get_size();
	}
};

int matrix_io_callback::invoke(safs::io_request *reqs[], int num)
{
	for (int i = 0; i < num; i++) {
		pending_size -= reqs[i]->get_size();
		task_map_t::const_iterator it = tasks.find(reqs[i]->get_buf());
		it->second->run(reqs[i]->get_buf(), reqs[i]->get_size());
		// Once a task is complete, we can remove it from the hashtable.
		tasks.erase(it);
	}
	return 0;
}

bool matrix_worker_thread::get_next_io(matrix_io &io)
{
	if (this_io_gen->has_next_io()) {
		io = this_io_gen->get_next_io();
		return io.is_valid();
	}
	else {
		for (size_t i = 0; i < io_gens.size(); i++) {
			if (io_gens[steal_io_id]->has_next_io()) {
				io = io_gens[steal_io_id]->steal_io();
				if (io.is_valid())
					return true;
			}
			steal_io_id = (steal_io_id + 1) % io_gens.size();
		}
		return false;
	}
}

void matrix_worker_thread::run()
{
	matrix_io_callback *cb = new matrix_io_callback();
	io->set_callback(cb);
	matrix_io mio;
	while (get_next_io(mio)) {
		compute_task::ptr task = tcreator->create(mio);
		safs::io_request req = task->get_request();
		cb->add_task(req, task);
		io->access(&req, 1);

		// TODO it might not be good to have a fixed number of pending I/O.
		// We need to control memory consumption for the buffers.
		while (io->num_pending_ios() > MAX_PENDING_IOS)
			io->wait4complete(1);
	}
	io->wait4complete(io->num_pending_ios());
	assert(io->num_pending_ios() == 0);
	stop();
	delete cb;
}

}
<commit_msg>[Matrix]: use shared_ptr for callback.<commit_after>/*
 * Copyright 2014 Open Connectome Project (http://openconnecto.me)
 * Written by Da Zheng (zhengda1936@gmail.com)
 *
 * This file is part of FlashMatrix.
 *
 * 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 <unordered_map>

#include "matrix_worker_thread.h"
#include "sparse_matrix.h"

namespace fm
{

static const int MAX_PENDING_IOS = 32;

class matrix_io_callback: public safs::callback
{
	typedef std::unordered_map<char *, compute_task::ptr> task_map_t;
	task_map_t tasks;
	size_t pending_size;
public:
	matrix_io_callback() {
		pending_size = 0;
	}

	~matrix_io_callback() {
		assert(pending_size == 0);
	}

	size_t get_pending_size() const {
		return pending_size;
	}

	size_t get_pending_ios() const {
		return tasks.size();
	}

	int invoke(safs::io_request *reqs[], int num);
	void add_task(const safs::io_request &req, compute_task::ptr task) {
		tasks.insert(task_map_t::value_type(req.get_buf(), task));
		pending_size += req.get_size();
	}
};

int matrix_io_callback::invoke(safs::io_request *reqs[], int num)
{
	for (int i = 0; i < num; i++) {
		pending_size -= reqs[i]->get_size();
		task_map_t::const_iterator it = tasks.find(reqs[i]->get_buf());
		it->second->run(reqs[i]->get_buf(), reqs[i]->get_size());
		// Once a task is complete, we can remove it from the hashtable.
		tasks.erase(it);
	}
	return 0;
}

bool matrix_worker_thread::get_next_io(matrix_io &io)
{
	if (this_io_gen->has_next_io()) {
		io = this_io_gen->get_next_io();
		return io.is_valid();
	}
	else {
		for (size_t i = 0; i < io_gens.size(); i++) {
			if (io_gens[steal_io_id]->has_next_io()) {
				io = io_gens[steal_io_id]->steal_io();
				if (io.is_valid())
					return true;
			}
			steal_io_id = (steal_io_id + 1) % io_gens.size();
		}
		return false;
	}
}

void matrix_worker_thread::run()
{
	matrix_io_callback *cb = new matrix_io_callback();
	io->set_callback(safs::callback::ptr(cb));
	matrix_io mio;
	while (get_next_io(mio)) {
		compute_task::ptr task = tcreator->create(mio);
		safs::io_request req = task->get_request();
		cb->add_task(req, task);
		io->access(&req, 1);

		// TODO it might not be good to have a fixed number of pending I/O.
		// We need to control memory consumption for the buffers.
		while (io->num_pending_ios() > MAX_PENDING_IOS)
			io->wait4complete(1);
	}
	io->wait4complete(io->num_pending_ios());
	assert(io->num_pending_ios() == 0);
	stop();
}

}
<|endoftext|>


<commit_before>// Copyright 2019 The MediaPipe 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 <utility>

#include "absl/memory/memory.h"
#include "absl/status/status.h"
#include "absl/status/statusor.h"
#include "mediapipe/framework/port/logging.h"
#include "mediapipe/framework/port/ret_check.h"
#include "mediapipe/framework/port/status.h"
#include "mediapipe/framework/port/status_builder.h"
#include "mediapipe/gpu/gl_context.h"
#include "mediapipe/gpu/gl_context_internal.h"

#ifndef EGL_OPENGL_ES3_BIT_KHR
#define EGL_OPENGL_ES3_BIT_KHR 0x00000040
#endif

#if HAS_EGL

namespace mediapipe {

namespace {

static pthread_key_t egl_release_thread_key;
static pthread_once_t egl_release_key_once = PTHREAD_ONCE_INIT;

static void EglThreadExitCallback(void* key_value) {
#if defined(__ANDROID__)
  eglMakeCurrent(EGL_NO_DISPLAY, EGL_NO_SURFACE, EGL_NO_SURFACE,
                 EGL_NO_CONTEXT);
#else
  // Some implementations have chosen to allow EGL_NO_DISPLAY as a valid display
  // parameter for eglMakeCurrent. This behavior is not portable to all EGL
  // implementations, and should be considered as an undocumented vendor
  // extension.
  // https://www.khronos.org/registry/EGL/sdk/docs/man/html/eglMakeCurrent.xhtml
  //
  // NOTE: crashes on some Android devices (occurs with libGLES_meow.so).
  eglMakeCurrent(eglGetDisplay(EGL_DEFAULT_DISPLAY), EGL_NO_SURFACE,
                 EGL_NO_SURFACE, EGL_NO_CONTEXT);
#endif
  eglReleaseThread();
}

// If a key has a destructor callback, and a thread has a non-NULL value for
// that key, then the destructor is called when the thread exits.
static void MakeEglReleaseThreadKey() {
  int err = pthread_key_create(&egl_release_thread_key, EglThreadExitCallback);
  if (err) {
    LOG(ERROR) << "cannot create pthread key: " << err;
  }
}

// This function can be called any number of times. For any thread on which it
// was called at least once, the EglThreadExitCallback will be called (once)
// when the thread exits.
static void EnsureEglThreadRelease() {
  pthread_once(&egl_release_key_once, MakeEglReleaseThreadKey);
  pthread_setspecific(egl_release_thread_key,
                      reinterpret_cast<void*>(0xDEADBEEF));
}

static absl::StatusOr<EGLDisplay> GetInitializedDefaultEglDisplay() {
  EGLDisplay display = eglGetDisplay(EGL_DEFAULT_DISPLAY);
  RET_CHECK(display != EGL_NO_DISPLAY)
      << "eglGetDisplay() returned error " << std::showbase << std::hex
      << eglGetError();

  EGLint major = 0;
  EGLint minor = 0;
  EGLBoolean egl_initialized = eglInitialize(display, &major, &minor);
  RET_CHECK(egl_initialized) << "Unable to initialize EGL";
  LOG(INFO) << "Successfully initialized EGL. Major : " << major
            << " Minor: " << minor;

  return display;
}

static absl::StatusOr<EGLDisplay> GetInitializedEglDisplay() {
  auto status_or_display = GetInitializedDefaultEglDisplay();
  return status_or_display;
}

}  // namespace

GlContext::StatusOrGlContext GlContext::Create(std::nullptr_t nullp,
                                               bool create_thread) {
  return Create(EGL_NO_CONTEXT, create_thread);
}

GlContext::StatusOrGlContext GlContext::Create(const GlContext& share_context,
                                               bool create_thread) {
  return Create(share_context.context_, create_thread);
}

GlContext::StatusOrGlContext GlContext::Create(EGLContext share_context,
                                               bool create_thread) {
  std::shared_ptr<GlContext> context(new GlContext());
  MP_RETURN_IF_ERROR(context->CreateContext(share_context));
  MP_RETURN_IF_ERROR(context->FinishInitialization(create_thread));
  return std::move(context);
}

absl::Status GlContext::CreateContextInternal(EGLContext share_context,
                                              int gl_version) {
  CHECK(gl_version == 2 || gl_version == 3);

  const EGLint config_attr[] = {
      // clang-format off
      EGL_RENDERABLE_TYPE, gl_version == 3 ? EGL_OPENGL_ES3_BIT_KHR
                                           : EGL_OPENGL_ES2_BIT,
      // Allow rendering to pixel buffers or directly to windows.
      EGL_SURFACE_TYPE,
#ifdef MEDIAPIPE_OMIT_EGL_WINDOW_BIT
      EGL_PBUFFER_BIT,
#else
      EGL_PBUFFER_BIT | EGL_WINDOW_BIT,
#endif
      EGL_RED_SIZE, 8,
      EGL_GREEN_SIZE, 8,
      EGL_BLUE_SIZE, 8,
      EGL_ALPHA_SIZE, 8,  // if you need the alpha channel
      EGL_DEPTH_SIZE, 16,  // if you need the depth buffer
      EGL_NONE
      // clang-format on
  };

  // TODO: improve config selection.
  EGLint num_configs;
  EGLBoolean success =
      eglChooseConfig(display_, config_attr, &config_, 1, &num_configs);
  if (!success) {
    return ::mediapipe::UnknownErrorBuilder(MEDIAPIPE_LOC)
           << "eglChooseConfig() returned error " << std::showbase << std::hex
           << eglGetError();
  }
  if (!num_configs) {
    return mediapipe::UnknownErrorBuilder(MEDIAPIPE_LOC)
           << "eglChooseConfig() returned no matching EGL configuration for "
           << "RGBA8888 D16 ES" << gl_version << " request. ";
  }

  const EGLint context_attr[] = {
      // clang-format off
      EGL_CONTEXT_CLIENT_VERSION, gl_version,
      EGL_NONE
      // clang-format on
  };

  context_ = eglCreateContext(display_, config_, share_context, context_attr);
  int error = eglGetError();
  RET_CHECK(context_ != EGL_NO_CONTEXT)
      << "Could not create GLES " << gl_version << " context; "
      << "eglCreateContext() returned error " << std::showbase << std::hex
      << error
      << (error == EGL_BAD_CONTEXT
              ? ": external context uses a different version of OpenGL"
              : "");

  // We can't always rely on GL_MAJOR_VERSION and GL_MINOR_VERSION, since
  // GLES 2 does not have them, so let's set the major version here at least.
  gl_major_version_ = gl_version;

  return absl::OkStatus();
}

absl::Status GlContext::CreateContext(EGLContext share_context) {
  ASSIGN_OR_RETURN(display_, GetInitializedEglDisplay());

  auto status = CreateContextInternal(share_context, 3);
  if (!status.ok()) {
    LOG(WARNING) << "Creating a context with OpenGL ES 3 failed: " << status;
    LOG(WARNING) << "Fall back on OpenGL ES 2.";
    status = CreateContextInternal(share_context, 2);
  }
  MP_RETURN_IF_ERROR(status);

  EGLint pbuffer_attr[] = {EGL_WIDTH, 1, EGL_HEIGHT, 1, EGL_NONE};

  surface_ = eglCreatePbufferSurface(display_, config_, pbuffer_attr);
  RET_CHECK(surface_ != EGL_NO_SURFACE)
      << "eglCreatePbufferSurface() returned error " << std::showbase
      << std::hex << eglGetError();

  return absl::OkStatus();
}

void GlContext::DestroyContext() {
#ifdef __ANDROID__
  if (HasContext()) {
    // Detach the current program to work around b/166322604.
    auto detach_program = [this] {
      GlContext::ContextBinding saved_context;
      GetCurrentContextBinding(&saved_context);
      // Note: cannot use ThisContextBinding because it calls shared_from_this,
      // which is not available during destruction.
      if (eglMakeCurrent(display_, surface_, surface_, context_)) {
        glUseProgram(0);
      } else {
        LOG(ERROR) << "eglMakeCurrent() returned error " << std::showbase
                   << std::hex << eglGetError();
      }
      return SetCurrentContextBinding(saved_context);
    };
    auto status = thread_ ? thread_->Run(detach_program) : detach_program();
    LOG_IF(ERROR, !status.ok()) << status;
  }
#endif  // __ANDROID__

  if (thread_) {
    // Delete thread-local storage.
    // TODO: in theory our EglThreadExitCallback should suffice for
    // this; however, heapcheck still reports a leak without this call here
    // when using SwiftShader.
    // Perhaps heapcheck misses the thread destructors?
    thread_
        ->Run([] {
          eglReleaseThread();
          return absl::OkStatus();
        })
        .IgnoreError();
  }

  // Destroy the context and surface.
  if (IsCurrent()) {
    if (!eglMakeCurrent(display_, EGL_NO_SURFACE, EGL_NO_SURFACE,
                        EGL_NO_CONTEXT)) {
      LOG(ERROR) << "eglMakeCurrent() returned error " << std::showbase
                 << std::hex << eglGetError();
    }
  }
  if (surface_ != EGL_NO_SURFACE) {
    if (!eglDestroySurface(display_, surface_)) {
      LOG(ERROR) << "eglDestroySurface() returned error " << std::showbase
                 << std::hex << eglGetError();
    }
    surface_ = EGL_NO_SURFACE;
  }
  if (context_ != EGL_NO_CONTEXT) {
    if (!eglDestroyContext(display_, context_)) {
      LOG(ERROR) << "eglDestroyContext() returned error " << std::showbase
                 << std::hex << eglGetError();
    }
    context_ = EGL_NO_CONTEXT;
  }

  // Under standard EGL, eglTerminate will terminate the display connection
  // for the entire process, no matter how many times eglInitialize has been
  // called. So we do not want to terminate it here, in case someone else is
  // using it.
  // However, Android implements non-standard reference-counted semantics for
  // eglInitialize/eglTerminate, so we should call it on that platform.
#ifdef __ANDROID__
  // TODO: this is removed for now since it caused issues on
  // YouTube. But in theory we _should_ be calling it. Needs more
  // investigation.
  // eglTerminate(display_);
#endif  // __ANDROID__
}

GlContext::ContextBinding GlContext::ThisContextBindingPlatform() {
  GlContext::ContextBinding result;
  result.display = display_;
  result.draw_surface = surface_;
  result.read_surface = surface_;
  result.context = context_;
  return result;
}

void GlContext::GetCurrentContextBinding(GlContext::ContextBinding* binding) {
  binding->display = eglGetCurrentDisplay();
  binding->draw_surface = eglGetCurrentSurface(EGL_DRAW);
  binding->read_surface = eglGetCurrentSurface(EGL_READ);
  binding->context = eglGetCurrentContext();
}

absl::Status GlContext::SetCurrentContextBinding(
    const ContextBinding& new_binding) {
  EnsureEglThreadRelease();
  EGLDisplay display = new_binding.display;
  if (display == EGL_NO_DISPLAY) {
    display = eglGetCurrentDisplay();
  }
  if (display == EGL_NO_DISPLAY) {
    display = eglGetDisplay(EGL_DEFAULT_DISPLAY);
  }
  EGLBoolean success =
      eglMakeCurrent(display, new_binding.draw_surface,
                     new_binding.read_surface, new_binding.context);
  RET_CHECK(success) << "eglMakeCurrent() returned error " << std::showbase
                     << std::hex << eglGetError();
  return absl::OkStatus();
}

bool GlContext::HasContext() const { return context_ != EGL_NO_CONTEXT; }

bool GlContext::IsCurrent() const {
  return HasContext() && (eglGetCurrentContext() == context_);
}

}  // namespace mediapipe

#endif  // HAS_EGL
<commit_msg>Internal change<commit_after>// Copyright 2019 The MediaPipe 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 <utility>

#include "absl/memory/memory.h"
#include "absl/status/status.h"
#include "absl/status/statusor.h"
#include "mediapipe/framework/port/logging.h"
#include "mediapipe/framework/port/ret_check.h"
#include "mediapipe/framework/port/status.h"
#include "mediapipe/framework/port/status_builder.h"
#include "mediapipe/gpu/gl_context.h"
#include "mediapipe/gpu/gl_context_internal.h"

#ifndef EGL_OPENGL_ES3_BIT_KHR
#define EGL_OPENGL_ES3_BIT_KHR 0x00000040
#endif

#if HAS_EGL

namespace mediapipe {

namespace {

static pthread_key_t egl_release_thread_key;
static pthread_once_t egl_release_key_once = PTHREAD_ONCE_INIT;

static void EglThreadExitCallback(void* key_value) {
  EGLDisplay current_display = eglGetCurrentDisplay();
  if (current_display != EGL_NO_DISPLAY) {
    // Some implementations have chosen to allow EGL_NO_DISPLAY as a valid
    // display parameter for eglMakeCurrent. This behavior is not portable to
    // all EGL implementations, and should be considered as an undocumented
    // vendor extension.
    // https://www.khronos.org/registry/EGL/sdk/docs/man/html/eglMakeCurrent.xhtml
    // Instead, to release the current context, we pass the current display.
    // If the current display is already EGL_NO_DISPLAY, no context is current.
    eglMakeCurrent(current_display, EGL_NO_SURFACE, EGL_NO_SURFACE,
                   EGL_NO_CONTEXT);
  }
  eglReleaseThread();
}

// If a key has a destructor callback, and a thread has a non-NULL value for
// that key, then the destructor is called when the thread exits.
static void MakeEglReleaseThreadKey() {
  int err = pthread_key_create(&egl_release_thread_key, EglThreadExitCallback);
  if (err) {
    LOG(ERROR) << "cannot create pthread key: " << err;
  }
}

// This function can be called any number of times. For any thread on which it
// was called at least once, the EglThreadExitCallback will be called (once)
// when the thread exits.
static void EnsureEglThreadRelease() {
  pthread_once(&egl_release_key_once, MakeEglReleaseThreadKey);
  pthread_setspecific(egl_release_thread_key,
                      reinterpret_cast<void*>(0xDEADBEEF));
}

static absl::StatusOr<EGLDisplay> GetInitializedDefaultEglDisplay() {
  EGLDisplay display = eglGetDisplay(EGL_DEFAULT_DISPLAY);
  RET_CHECK(display != EGL_NO_DISPLAY)
      << "eglGetDisplay() returned error " << std::showbase << std::hex
      << eglGetError();

  EGLint major = 0;
  EGLint minor = 0;
  EGLBoolean egl_initialized = eglInitialize(display, &major, &minor);
  RET_CHECK(egl_initialized) << "Unable to initialize EGL";
  LOG(INFO) << "Successfully initialized EGL. Major : " << major
            << " Minor: " << minor;

  return display;
}

static absl::StatusOr<EGLDisplay> GetInitializedEglDisplay() {
  auto status_or_display = GetInitializedDefaultEglDisplay();
  return status_or_display;
}

}  // namespace

GlContext::StatusOrGlContext GlContext::Create(std::nullptr_t nullp,
                                               bool create_thread) {
  return Create(EGL_NO_CONTEXT, create_thread);
}

GlContext::StatusOrGlContext GlContext::Create(const GlContext& share_context,
                                               bool create_thread) {
  return Create(share_context.context_, create_thread);
}

GlContext::StatusOrGlContext GlContext::Create(EGLContext share_context,
                                               bool create_thread) {
  std::shared_ptr<GlContext> context(new GlContext());
  MP_RETURN_IF_ERROR(context->CreateContext(share_context));
  MP_RETURN_IF_ERROR(context->FinishInitialization(create_thread));
  return std::move(context);
}

absl::Status GlContext::CreateContextInternal(EGLContext share_context,
                                              int gl_version) {
  CHECK(gl_version == 2 || gl_version == 3);

  const EGLint config_attr[] = {
      // clang-format off
      EGL_RENDERABLE_TYPE, gl_version == 3 ? EGL_OPENGL_ES3_BIT_KHR
                                           : EGL_OPENGL_ES2_BIT,
      // Allow rendering to pixel buffers or directly to windows.
      EGL_SURFACE_TYPE,
#ifdef MEDIAPIPE_OMIT_EGL_WINDOW_BIT
      EGL_PBUFFER_BIT,
#else
      EGL_PBUFFER_BIT | EGL_WINDOW_BIT,
#endif
      EGL_RED_SIZE, 8,
      EGL_GREEN_SIZE, 8,
      EGL_BLUE_SIZE, 8,
      EGL_ALPHA_SIZE, 8,  // if you need the alpha channel
      EGL_DEPTH_SIZE, 16,  // if you need the depth buffer
      EGL_NONE
      // clang-format on
  };

  // TODO: improve config selection.
  EGLint num_configs;
  EGLBoolean success =
      eglChooseConfig(display_, config_attr, &config_, 1, &num_configs);
  if (!success) {
    return ::mediapipe::UnknownErrorBuilder(MEDIAPIPE_LOC)
           << "eglChooseConfig() returned error " << std::showbase << std::hex
           << eglGetError();
  }
  if (!num_configs) {
    return mediapipe::UnknownErrorBuilder(MEDIAPIPE_LOC)
           << "eglChooseConfig() returned no matching EGL configuration for "
           << "RGBA8888 D16 ES" << gl_version << " request. ";
  }

  const EGLint context_attr[] = {
      // clang-format off
      EGL_CONTEXT_CLIENT_VERSION, gl_version,
      EGL_NONE
      // clang-format on
  };

  context_ = eglCreateContext(display_, config_, share_context, context_attr);
  int error = eglGetError();
  RET_CHECK(context_ != EGL_NO_CONTEXT)
      << "Could not create GLES " << gl_version << " context; "
      << "eglCreateContext() returned error " << std::showbase << std::hex
      << error
      << (error == EGL_BAD_CONTEXT
              ? ": external context uses a different version of OpenGL"
              : "");

  // We can't always rely on GL_MAJOR_VERSION and GL_MINOR_VERSION, since
  // GLES 2 does not have them, so let's set the major version here at least.
  gl_major_version_ = gl_version;

  return absl::OkStatus();
}

absl::Status GlContext::CreateContext(EGLContext share_context) {
  ASSIGN_OR_RETURN(display_, GetInitializedEglDisplay());

  auto status = CreateContextInternal(share_context, 3);
  if (!status.ok()) {
    LOG(WARNING) << "Creating a context with OpenGL ES 3 failed: " << status;
    LOG(WARNING) << "Fall back on OpenGL ES 2.";
    status = CreateContextInternal(share_context, 2);
  }
  MP_RETURN_IF_ERROR(status);

  EGLint pbuffer_attr[] = {EGL_WIDTH, 1, EGL_HEIGHT, 1, EGL_NONE};

  surface_ = eglCreatePbufferSurface(display_, config_, pbuffer_attr);
  RET_CHECK(surface_ != EGL_NO_SURFACE)
      << "eglCreatePbufferSurface() returned error " << std::showbase
      << std::hex << eglGetError();

  return absl::OkStatus();
}

void GlContext::DestroyContext() {
#ifdef __ANDROID__
  if (HasContext()) {
    // Detach the current program to work around b/166322604.
    auto detach_program = [this] {
      GlContext::ContextBinding saved_context;
      GetCurrentContextBinding(&saved_context);
      // Note: cannot use ThisContextBinding because it calls shared_from_this,
      // which is not available during destruction.
      if (eglMakeCurrent(display_, surface_, surface_, context_)) {
        glUseProgram(0);
      } else {
        LOG(ERROR) << "eglMakeCurrent() returned error " << std::showbase
                   << std::hex << eglGetError();
      }
      return SetCurrentContextBinding(saved_context);
    };
    auto status = thread_ ? thread_->Run(detach_program) : detach_program();
    LOG_IF(ERROR, !status.ok()) << status;
  }
#endif  // __ANDROID__

  if (thread_) {
    // Delete thread-local storage.
    // TODO: in theory our EglThreadExitCallback should suffice for
    // this; however, heapcheck still reports a leak without this call here
    // when using SwiftShader.
    // Perhaps heapcheck misses the thread destructors?
    thread_
        ->Run([] {
          eglReleaseThread();
          return absl::OkStatus();
        })
        .IgnoreError();
  }

  // Destroy the context and surface.
  if (IsCurrent()) {
    if (!eglMakeCurrent(display_, EGL_NO_SURFACE, EGL_NO_SURFACE,
                        EGL_NO_CONTEXT)) {
      LOG(ERROR) << "eglMakeCurrent() returned error " << std::showbase
                 << std::hex << eglGetError();
    }
  }
  if (surface_ != EGL_NO_SURFACE) {
    if (!eglDestroySurface(display_, surface_)) {
      LOG(ERROR) << "eglDestroySurface() returned error " << std::showbase
                 << std::hex << eglGetError();
    }
    surface_ = EGL_NO_SURFACE;
  }
  if (context_ != EGL_NO_CONTEXT) {
    if (!eglDestroyContext(display_, context_)) {
      LOG(ERROR) << "eglDestroyContext() returned error " << std::showbase
                 << std::hex << eglGetError();
    }
    context_ = EGL_NO_CONTEXT;
  }

  // Under standard EGL, eglTerminate will terminate the display connection
  // for the entire process, no matter how many times eglInitialize has been
  // called. So we do not want to terminate it here, in case someone else is
  // using it.
  // However, Android implements non-standard reference-counted semantics for
  // eglInitialize/eglTerminate, so we should call it on that platform.
#ifdef __ANDROID__
  // TODO: this is removed for now since it caused issues on
  // YouTube. But in theory we _should_ be calling it. Needs more
  // investigation.
  // eglTerminate(display_);
#endif  // __ANDROID__
}

GlContext::ContextBinding GlContext::ThisContextBindingPlatform() {
  GlContext::ContextBinding result;
  result.display = display_;
  result.draw_surface = surface_;
  result.read_surface = surface_;
  result.context = context_;
  return result;
}

void GlContext::GetCurrentContextBinding(GlContext::ContextBinding* binding) {
  binding->display = eglGetCurrentDisplay();
  binding->draw_surface = eglGetCurrentSurface(EGL_DRAW);
  binding->read_surface = eglGetCurrentSurface(EGL_READ);
  binding->context = eglGetCurrentContext();
}

absl::Status GlContext::SetCurrentContextBinding(
    const ContextBinding& new_binding) {
  EnsureEglThreadRelease();
  EGLDisplay display = new_binding.display;
  if (display == EGL_NO_DISPLAY) {
    display = eglGetCurrentDisplay();
  }
  if (display == EGL_NO_DISPLAY) {
    display = eglGetDisplay(EGL_DEFAULT_DISPLAY);
  }
  EGLBoolean success =
      eglMakeCurrent(display, new_binding.draw_surface,
                     new_binding.read_surface, new_binding.context);
  RET_CHECK(success) << "eglMakeCurrent() returned error " << std::showbase
                     << std::hex << eglGetError();
  return absl::OkStatus();
}

bool GlContext::HasContext() const { return context_ != EGL_NO_CONTEXT; }

bool GlContext::IsCurrent() const {
  return HasContext() && (eglGetCurrentContext() == context_);
}

}  // namespace mediapipe

#endif  // HAS_EGL
<|endoftext|>