surround_view/service-impl/SurroundView3dSession.cpp - platform/packages/services/Car - Gitiles

 /*
  * Copyright 2020 The Android Open Source Project
  *
  * 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.
  */
 #define LOG_TAG "SurroundViewService"

 #include <android-base/logging.h>
 #include <android/hardware_buffer.h>
 #include <android/hidl/memory/1.0/IMemory.h>
 #include <hidlmemory/mapping.h>
 #include <set>
 #include <utils/SystemClock.h>

 #include "SurroundView3dSession.h"
 #include "sv_3d_params.h"

 using ::android::hidl::memory::V1_0::IMemory;
 using ::android::hardware::hidl_memory;

 namespace android {
 namespace hardware {
 namespace automotive {
 namespace sv {
 namespace V1_0 {
 namespace implementation {

 static const char kGrayColor = 128;
 static const int kNumChannels = 4;

 SurroundView3dSession::SurroundView3dSession() :
     mStreamState(STOPPED){
     mEvsCameraIds = {"0" , "1", "2", "3"};
 }

 // Methods from ::android::hardware::automotive::sv::V1_0::ISurroundViewSession.
 Return<SvResult> SurroundView3dSession::startStream(
     const sp<ISurroundViewStream>& stream) {
     LOG(DEBUG) << __FUNCTION__;
     scoped_lock<mutex> lock(mAccessLock);

     if (!mIsInitialized && !initialize()) {
         LOG(ERROR) << "There is an error while initializing the use case. "
                    << "Exiting";
         return SvResult::INTERNAL_ERROR;
     }

     if (mStreamState != STOPPED) {
         LOG(ERROR) << "Ignoring startVideoStream call when a stream is "
                    << "already running.";
         return SvResult::INTERNAL_ERROR;
     }

     if (mViews.empty()) {
         LOG(ERROR) << "No views have been set for current Surround View"
                    << "3d Session. Please call setViews before starting"
                    << "the stream.";
         return SvResult::VIEW_NOT_SET;
     }

     if (stream == nullptr) {
         LOG(ERROR) << "The input stream is invalid";
         return SvResult::INTERNAL_ERROR;
     }
     mStream = stream;

     LOG(DEBUG) << "Notify SvEvent::STREAM_STARTED";
     mStream->notify(SvEvent::STREAM_STARTED);

     // Start the frame generation thread
     mStreamState = RUNNING;
     mCaptureThread = thread([this](){
         generateFrames();
     });

     return SvResult::OK;
 }

 Return<void> SurroundView3dSession::stopStream() {
     LOG(DEBUG) << __FUNCTION__;
     unique_lock <mutex> lock(mAccessLock);

     if (mStreamState == RUNNING) {
         // Tell the GenerateFrames loop we want it to stop
         mStreamState = STOPPING;

         // Block outside the mutex until the "stop" flag has been acknowledged
         // We won't send any more frames, but the client might still get some
         // already in flight
         LOG(DEBUG) << __FUNCTION__ << ": Waiting for stream thread to end...";
         lock.unlock();
         mCaptureThread.join();
         lock.lock();

         mStreamState = STOPPED;
         mStream = nullptr;
         LOG(DEBUG) << "Stream marked STOPPED.";
     }

     return {};
 }

 Return<void> SurroundView3dSession::doneWithFrames(
     const SvFramesDesc& svFramesDesc){
     LOG(DEBUG) << __FUNCTION__;
     scoped_lock <mutex> lock(mAccessLock);

     framesRecord.inUse = false;

     (void)svFramesDesc;
     return {};
 }

 // Methods from ISurroundView3dSession follow.
 Return<SvResult> SurroundView3dSession::setViews(
     const hidl_vec<View3d>& views) {
     LOG(DEBUG) << __FUNCTION__;
     scoped_lock <mutex> lock(mAccessLock);

     mViews.resize(views.size());
     for (int i=0; i<views.size(); i++) {
         mViews[i] = views[i];
     }

     return SvResult::OK;
 }

 Return<SvResult> SurroundView3dSession::set3dConfig(const Sv3dConfig& sv3dConfig) {
     LOG(DEBUG) << __FUNCTION__;
     scoped_lock <mutex> lock(mAccessLock);

     if (sv3dConfig.width <=0 || sv3dConfig.width > 4096) {
         LOG(WARNING) << "The width of 3d config is out of the range (0, 4096]"
                      << "Ignored!";
         return SvResult::INVALID_ARG;
     }

     if (sv3dConfig.height <=0 || sv3dConfig.height > 4096) {
         LOG(WARNING) << "The height of 3d config is out of the range (0, 4096]"
                      << "Ignored!";
         return SvResult::INVALID_ARG;
     }

     mConfig.width = sv3dConfig.width;
     mConfig.height = sv3dConfig.height;
     mConfig.carDetails = sv3dConfig.carDetails;

     if (mStream != nullptr) {
         LOG(DEBUG) << "Notify SvEvent::CONFIG_UPDATED";
         mStream->notify(SvEvent::CONFIG_UPDATED);
     }

     return SvResult::OK;
 }

 Return<void> SurroundView3dSession::get3dConfig(get3dConfig_cb _hidl_cb) {
     LOG(DEBUG) << __FUNCTION__;

     _hidl_cb(mConfig);
     return {};
 }

 bool VerifyOverlayData(const OverlaysData& overlaysData) {
     // Check size of shared memory matches overlaysMemoryDesc.
     const int kVertexSize = 16;
     const int kIdSize = 2;
     int memDescSize = 0;
     for (auto& overlayMemDesc : overlaysData.overlaysMemoryDesc) {
         memDescSize += kIdSize + kVertexSize * overlayMemDesc.verticesCount;
     }
     if (memDescSize != overlaysData.overlaysMemory.size()) {
         LOG(ERROR) << "shared memory and overlaysMemoryDesc size mismatch.";
         return false;
     }

     // Map memory.
     sp<IMemory> pSharedMemory = mapMemory(overlaysData.overlaysMemory);
     if(pSharedMemory == nullptr) {
         LOG(ERROR) << "mapMemory failed.";
         return false;
     }

     // Get Data pointer.
     uint8_t* pData = static_cast<uint8_t*>(
         static_cast<void*>(pSharedMemory->getPointer()));
     if (pData == nullptr) {
         LOG(ERROR) << "Shared memory getPointer() failed.";
         return false;
     }

     int idOffset = 0;
     set<uint16_t> overlayIdSet;
     for (auto& overlayMemDesc : overlaysData.overlaysMemoryDesc) {

         if (overlayIdSet.find(overlayMemDesc.id) != overlayIdSet.end()) {
             LOG(ERROR) << "Duplicate id within memory descriptor.";
             return false;
         }
         overlayIdSet.insert(overlayMemDesc.id);

         if(overlayMemDesc.verticesCount < 3) {
             LOG(ERROR) << "Less than 3 vertices.";
             return false;
         }

         if (overlayMemDesc.overlayPrimitive == OverlayPrimitive::TRIANGLES &&
                 overlayMemDesc.verticesCount % 3 != 0) {
             LOG(ERROR) << "Triangles primitive does not have vertices "
                        << "multiple of 3.";
             return false;
         }

         const uint16_t overlayId = *((uint16_t*)(pData + idOffset));

         if (overlayId != overlayMemDesc.id) {
             LOG(ERROR) << "Overlay id mismatch "
                        << overlayId
                        << ", "
                        << overlayMemDesc.id;
             return false;
         }

         idOffset += kIdSize + (kVertexSize * overlayMemDesc.verticesCount);
     }

     return true;
 }

 // TODO(b/150412555): the overlay related methods are incomplete.
 Return<SvResult>  SurroundView3dSession::updateOverlays(
         const OverlaysData& overlaysData) {

     if(!VerifyOverlayData(overlaysData)) {
         LOG(ERROR) << "VerifyOverlayData failed.";
         return SvResult::INVALID_ARG;
     }

     return SvResult::OK;
 }

 Return<void> SurroundView3dSession::projectCameraPointsTo3dSurface(
     const hidl_vec<Point2dInt>& cameraPoints,
     const hidl_string& cameraId,
     projectCameraPointsTo3dSurface_cb _hidl_cb) {

     vector<Point3dFloat> points3d;
     bool cameraIdFound = false;
     for (auto& evsCameraId : mEvsCameraIds) {
       if (cameraId == evsCameraId) {
           cameraIdFound = true;
           LOG(INFO) << "Camera id found.";
           break;
       }
     }

     if (!cameraIdFound) {
         LOG(ERROR) << "Camera id not found.";
         _hidl_cb(points3d);
         return {};
     }

     for (const auto& cameraPoint : cameraPoints) {
         Point3dFloat point3d;
         point3d.isValid = (cameraPoint.x >= 0
                            && cameraPoint.x < mConfig.width
                            && cameraPoint.y >= 0
                            && cameraPoint.y < mConfig.height);
         if (!point3d.isValid) {
             LOG(WARNING) << "Camera point out of bounds.";
         }
         points3d.push_back(point3d);
     }
     _hidl_cb(points3d);
     return {};
 }

 void SurroundView3dSession::generateFrames() {
     int sequenceId = 0;

     // TODO(b/150412555): do not use the setViews for frames generation
     // since there is a discrepancy between the HIDL APIs and core lib APIs.
     vector<vector<float>> matrix;
     matrix.resize(4);
     for (int i=0; i<4; i++) {
         matrix[i].resize(4);
     }

     while(true) {
         {
             scoped_lock<mutex> lock(mAccessLock);

             if (mStreamState != RUNNING) {
                 // Break out of our main thread loop
                 LOG(INFO) << "StreamState does not equal to RUNNING. "
                           << "Exiting the loop";
                 break;
             }

             if (mOutputWidth != mConfig.width
                 || mOutputHeight != mConfig.height) {
                 LOG(DEBUG) << "Config changed. Re-allocate memory. "
                            << "Old width: "
                            << mOutputWidth
                            << ", old height: "
                            << mOutputHeight
                            << "; New width: "
                            << mConfig.width
                            << ", new height: "
                            << mConfig.height;
                 delete[] static_cast<char*>(mOutputPointer.data_pointer);
                 mOutputWidth = mConfig.width;
                 mOutputHeight = mConfig.height;
                 mOutputPointer.height = mOutputHeight;
                 mOutputPointer.width = mOutputWidth;
                 mOutputPointer.format = Format::RGBA;
                 mOutputPointer.data_pointer =
                     new char[mOutputHeight * mOutputWidth * kNumChannels];

                 if (!mOutputPointer.data_pointer) {
                     LOG(ERROR) << "Memory allocation failed. Exiting.";
                     break;
                 }

                 Size2dInteger size = Size2dInteger(mOutputWidth, mOutputHeight);
                 mSurroundView->Update3dOutputResolution(size);

                 mSvTexture = new GraphicBuffer(mOutputWidth,
                                                mOutputHeight,
                                                HAL_PIXEL_FORMAT_RGBA_8888,
                                                1,
                                                GRALLOC_USAGE_HW_TEXTURE,
                                                "SvTexture");
                 if (mSvTexture->initCheck() == OK) {
                     LOG(INFO) << "Successfully allocated Graphic Buffer";
                 } else {
                     LOG(ERROR) << "Failed to allocate Graphic Buffer";
                     break;
                 }
             }
         }

         // TODO(b/150412555): use hard-coded views for now. Change view every 10
         // frames.
         int recViewId = sequenceId / 10 % 16;
         for (int i=0; i<4; i++)
             for (int j=0; j<4; j++) {
                 matrix[i][j] = kRecViews[recViewId][i*4+j];
         }

         if (mSurroundView->Get3dSurroundView(
             mInputPointers, matrix, &mOutputPointer)) {
             LOG(INFO) << "Get3dSurroundView succeeded";
         } else {
             LOG(ERROR) << "Get3dSurroundView failed. "
                        << "Using memset to initialize to gray.";
             memset(mOutputPointer.data_pointer, kGrayColor,
                    mOutputHeight * mOutputWidth * kNumChannels);
         }

         void* textureDataPtr = nullptr;
         mSvTexture->lock(GRALLOC_USAGE_SW_WRITE_OFTEN
                         | GRALLOC_USAGE_SW_READ_NEVER,
                         &textureDataPtr);
         if (!textureDataPtr) {
             LOG(ERROR) << "Failed to gain write access to GraphicBuffer!";
             break;
         }

         // Note: there is a chance that the stride of the texture is not the
         // same as the width. For example, when the input frame is 1920 * 1080,
         // the width is 1080, but the stride is 2048. So we'd better copy the
         // data line by line, instead of single memcpy.
         uint8_t* writePtr = static_cast<uint8_t*>(textureDataPtr);
         uint8_t* readPtr = static_cast<uint8_t*>(mOutputPointer.data_pointer);
         const int readStride = mOutputWidth * kNumChannels;
         const int writeStride = mSvTexture->getStride() * kNumChannels;
         if (readStride == writeStride) {
             memcpy(writePtr, readPtr, readStride * mSvTexture->getHeight());
         } else {
             for (int i=0; i<mSvTexture->getHeight(); i++) {
                 memcpy(writePtr, readPtr, readStride);
                 writePtr = writePtr + writeStride;
                 readPtr = readPtr + readStride;
             }
         }
         LOG(INFO) << "memcpy finished!";
         mSvTexture->unlock();

         ANativeWindowBuffer* buffer = mSvTexture->getNativeBuffer();
         LOG(DEBUG) << "ANativeWindowBuffer->handle: " << buffer->handle;

         framesRecord.frames.svBuffers.resize(1);
         SvBuffer& svBuffer = framesRecord.frames.svBuffers[0];
         svBuffer.viewId = 0;
         svBuffer.hardwareBuffer.nativeHandle = buffer->handle;
         AHardwareBuffer_Desc* pDesc =
             reinterpret_cast<AHardwareBuffer_Desc *>(
                 &svBuffer.hardwareBuffer.description);
         pDesc->width = mOutputWidth;
         pDesc->height = mOutputHeight;
         pDesc->layers = 1;
         pDesc->usage = GRALLOC_USAGE_HW_TEXTURE;
         pDesc->stride = mSvTexture->getStride();
         pDesc->format = HAL_PIXEL_FORMAT_RGBA_8888;
         framesRecord.frames.timestampNs = elapsedRealtimeNano();
         framesRecord.frames.sequenceId = sequenceId++;

         {
             scoped_lock<mutex> lock(mAccessLock);

             if (framesRecord.inUse) {
                 LOG(DEBUG) << "Notify SvEvent::FRAME_DROPPED";
                 mStream->notify(SvEvent::FRAME_DROPPED);
             } else {
                 framesRecord.inUse = true;
                 mStream->receiveFrames(framesRecord.frames);
             }
         }
     }

     // If we've been asked to stop, send an event to signal the actual end of stream
     LOG(DEBUG) << "Notify SvEvent::STREAM_STOPPED";
     mStream->notify(SvEvent::STREAM_STOPPED);
 }

 bool SurroundView3dSession::initialize() {
     lock_guard<mutex> lock(mAccessLock, adopt_lock);

     // TODO(b/150412555): ask core-lib team to add API description for "create"
     // method in the .h file.
     // The create method will never return a null pointer based the API
     // description.
     mSurroundView = unique_ptr<SurroundView>(Create());

     mSurroundView->SetStaticData(GetCameras(), Get2dParams(), Get3dParams(),
                                  GetUndistortionScales(), GetBoundingBox());

     // TODO(b/150412555): remove after EVS camera is used
     mInputPointers = mSurroundView->ReadImages(
         "/etc/automotive/sv/cam0.png",
         "/etc/automotive/sv/cam1.png",
         "/etc/automotive/sv/cam2.png",
         "/etc/automotive/sv/cam3.png");
     if (mInputPointers.size() == 4
         && mInputPointers[0].cpu_data_pointer != nullptr) {
         LOG(INFO) << "ReadImages succeeded";
     } else {
         LOG(ERROR) << "Failed to read images";
         return false;
     }

     mOutputWidth = Get3dParams().resolution.width;
     mOutputHeight = Get3dParams().resolution.height;

     mConfig.width = mOutputWidth;
     mConfig.height = mOutputHeight;
     mConfig.carDetails = SvQuality::HIGH;

     mOutputPointer.height = mOutputHeight;
     mOutputPointer.width = mOutputWidth;
     mOutputPointer.format = Format::RGBA;
     mOutputPointer.data_pointer = new char[
         mOutputHeight * mOutputWidth * kNumChannels];

     if (!mOutputPointer.data_pointer) {
         LOG(ERROR) << "Memory allocation failed. Exiting.";
         return false;
     }

     mSvTexture = new GraphicBuffer(mOutputWidth,
                                    mOutputHeight,
                                    HAL_PIXEL_FORMAT_RGBA_8888,
                                    1,
                                    GRALLOC_USAGE_HW_TEXTURE,
                                    "SvTexture");

     if (mSvTexture->initCheck() == OK) {
         LOG(INFO) << "Successfully allocated Graphic Buffer";
     } else {
         LOG(ERROR) << "Failed to allocate Graphic Buffer";
         return false;
     }

     if (mSurroundView->Start3dPipeline()) {
         LOG(INFO) << "Start3dPipeline succeeded";
     } else {
         LOG(ERROR) << "Start3dPipeline failed";
         return false;
     }

     mIsInitialized = true;
     return true;
 }

 }  // namespace implementation
 }  // namespace V1_0
 }  // namespace sv
 }  // namespace automotive
 }  // namespace hardware
 }  // namespace android
	/*
	* Copyright 2020 The Android Open Source Project
	*
	* 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.
	*/
	#define LOG_TAG "SurroundViewService"

	#include <android-base/logging.h>
	#include <android/hardware_buffer.h>
	#include <android/hidl/memory/1.0/IMemory.h>
	#include <hidlmemory/mapping.h>
	#include <set>
	#include <utils/SystemClock.h>

	#include "SurroundView3dSession.h"
	#include "sv_3d_params.h"

	using ::android::hidl::memory::V1_0::IMemory;
	using ::android::hardware::hidl_memory;

	namespace android {
	namespace hardware {
	namespace automotive {
	namespace sv {
	namespace V1_0 {
	namespace implementation {

	static const char kGrayColor = 128;
	static const int kNumChannels = 4;

	SurroundView3dSession::SurroundView3dSession() :
	mStreamState(STOPPED){
	mEvsCameraIds = {"0" , "1", "2", "3"};
	}

	// Methods from ::android::hardware::automotive::sv::V1_0::ISurroundViewSession.
	Return<SvResult> SurroundView3dSession::startStream(
	const sp<ISurroundViewStream>& stream) {
	LOG(DEBUG) << __FUNCTION__;
	scoped_lock<mutex> lock(mAccessLock);

	if (!mIsInitialized && !initialize()) {
	LOG(ERROR) << "There is an error while initializing the use case. "
	<< "Exiting";
	return SvResult::INTERNAL_ERROR;
	}

	if (mStreamState != STOPPED) {
	LOG(ERROR) << "Ignoring startVideoStream call when a stream is "
	<< "already running.";
	return SvResult::INTERNAL_ERROR;
	}

	if (mViews.empty()) {
	LOG(ERROR) << "No views have been set for current Surround View"
	<< "3d Session. Please call setViews before starting"
	<< "the stream.";
	return SvResult::VIEW_NOT_SET;
	}

	if (stream == nullptr) {
	LOG(ERROR) << "The input stream is invalid";
	return SvResult::INTERNAL_ERROR;
	}
	mStream = stream;

	LOG(DEBUG) << "Notify SvEvent::STREAM_STARTED";
	mStream->notify(SvEvent::STREAM_STARTED);

	// Start the frame generation thread
	mStreamState = RUNNING;
	mCaptureThread = thread([this](){
	generateFrames();
	});

	return SvResult::OK;
	}

	Return<void> SurroundView3dSession::stopStream() {
	LOG(DEBUG) << __FUNCTION__;
	unique_lock <mutex> lock(mAccessLock);

	if (mStreamState == RUNNING) {
	// Tell the GenerateFrames loop we want it to stop
	mStreamState = STOPPING;

	// Block outside the mutex until the "stop" flag has been acknowledged
	// We won't send any more frames, but the client might still get some
	// already in flight
	LOG(DEBUG) << __FUNCTION__ << ": Waiting for stream thread to end...";
	lock.unlock();
	mCaptureThread.join();
	lock.lock();

	mStreamState = STOPPED;
	mStream = nullptr;
	LOG(DEBUG) << "Stream marked STOPPED.";
	}

	return {};
	}

	Return<void> SurroundView3dSession::doneWithFrames(
	const SvFramesDesc& svFramesDesc){
	LOG(DEBUG) << __FUNCTION__;
	scoped_lock <mutex> lock(mAccessLock);

	framesRecord.inUse = false;

	(void)svFramesDesc;
	return {};
	}

	// Methods from ISurroundView3dSession follow.
	Return<SvResult> SurroundView3dSession::setViews(
	const hidl_vec<View3d>& views) {
	LOG(DEBUG) << __FUNCTION__;
	scoped_lock <mutex> lock(mAccessLock);

	mViews.resize(views.size());
	for (int i=0; i<views.size(); i++) {
	mViews[i] = views[i];
	}

	return SvResult::OK;
	}

	Return<SvResult> SurroundView3dSession::set3dConfig(const Sv3dConfig& sv3dConfig) {
	LOG(DEBUG) << __FUNCTION__;
	scoped_lock <mutex> lock(mAccessLock);

	if (sv3dConfig.width <=0 \|\| sv3dConfig.width > 4096) {
	LOG(WARNING) << "The width of 3d config is out of the range (0, 4096]"
	<< "Ignored!";
	return SvResult::INVALID_ARG;
	}

	if (sv3dConfig.height <=0 \|\| sv3dConfig.height > 4096) {
	LOG(WARNING) << "The height of 3d config is out of the range (0, 4096]"
	<< "Ignored!";
	return SvResult::INVALID_ARG;
	}

	mConfig.width = sv3dConfig.width;
	mConfig.height = sv3dConfig.height;
	mConfig.carDetails = sv3dConfig.carDetails;

	if (mStream != nullptr) {
	LOG(DEBUG) << "Notify SvEvent::CONFIG_UPDATED";
	mStream->notify(SvEvent::CONFIG_UPDATED);
	}

	return SvResult::OK;
	}

	Return<void> SurroundView3dSession::get3dConfig(get3dConfig_cb _hidl_cb) {
	LOG(DEBUG) << __FUNCTION__;

	_hidl_cb(mConfig);
	return {};
	}

	bool VerifyOverlayData(const OverlaysData& overlaysData) {
	// Check size of shared memory matches overlaysMemoryDesc.
	const int kVertexSize = 16;
	const int kIdSize = 2;
	int memDescSize = 0;
	for (auto& overlayMemDesc : overlaysData.overlaysMemoryDesc) {
	memDescSize += kIdSize + kVertexSize * overlayMemDesc.verticesCount;
	}
	if (memDescSize != overlaysData.overlaysMemory.size()) {
	LOG(ERROR) << "shared memory and overlaysMemoryDesc size mismatch.";
	return false;
	}

	// Map memory.
	sp<IMemory> pSharedMemory = mapMemory(overlaysData.overlaysMemory);
	if(pSharedMemory == nullptr) {
	LOG(ERROR) << "mapMemory failed.";
	return false;
	}

	// Get Data pointer.
	uint8_t* pData = static_cast<uint8_t*>(
	static_cast<void*>(pSharedMemory->getPointer()));
	if (pData == nullptr) {
	LOG(ERROR) << "Shared memory getPointer() failed.";
	return false;
	}

	int idOffset = 0;
	set<uint16_t> overlayIdSet;
	for (auto& overlayMemDesc : overlaysData.overlaysMemoryDesc) {

	if (overlayIdSet.find(overlayMemDesc.id) != overlayIdSet.end()) {
	LOG(ERROR) << "Duplicate id within memory descriptor.";
	return false;
	}
	overlayIdSet.insert(overlayMemDesc.id);

	if(overlayMemDesc.verticesCount < 3) {
	LOG(ERROR) << "Less than 3 vertices.";
	return false;
	}

	if (overlayMemDesc.overlayPrimitive == OverlayPrimitive::TRIANGLES &&
	overlayMemDesc.verticesCount % 3 != 0) {
	LOG(ERROR) << "Triangles primitive does not have vertices "
	<< "multiple of 3.";
	return false;
	}

	const uint16_t overlayId = ((uint16_t)(pData + idOffset));

	if (overlayId != overlayMemDesc.id) {
	LOG(ERROR) << "Overlay id mismatch "
	<< overlayId
	<< ", "
	<< overlayMemDesc.id;
	return false;
	}

	idOffset += kIdSize + (kVertexSize * overlayMemDesc.verticesCount);
	}

	return true;
	}

	// TODO(b/150412555): the overlay related methods are incomplete.
	Return<SvResult> SurroundView3dSession::updateOverlays(
	const OverlaysData& overlaysData) {

	if(!VerifyOverlayData(overlaysData)) {
	LOG(ERROR) << "VerifyOverlayData failed.";
	return SvResult::INVALID_ARG;
	}

	return SvResult::OK;
	}

	Return<void> SurroundView3dSession::projectCameraPointsTo3dSurface(
	const hidl_vec<Point2dInt>& cameraPoints,
	const hidl_string& cameraId,
	projectCameraPointsTo3dSurface_cb _hidl_cb) {

	vector<Point3dFloat> points3d;
	bool cameraIdFound = false;
	for (auto& evsCameraId : mEvsCameraIds) {
	if (cameraId == evsCameraId) {
	cameraIdFound = true;
	LOG(INFO) << "Camera id found.";
	break;
	}
	}

	if (!cameraIdFound) {
	LOG(ERROR) << "Camera id not found.";
	_hidl_cb(points3d);
	return {};
	}

	for (const auto& cameraPoint : cameraPoints) {
	Point3dFloat point3d;
	point3d.isValid = (cameraPoint.x >= 0
	&& cameraPoint.x < mConfig.width
	&& cameraPoint.y >= 0
	&& cameraPoint.y < mConfig.height);
	if (!point3d.isValid) {
	LOG(WARNING) << "Camera point out of bounds.";
	}
	points3d.push_back(point3d);
	}
	_hidl_cb(points3d);
	return {};
	}

	void SurroundView3dSession::generateFrames() {
	int sequenceId = 0;

	// TODO(b/150412555): do not use the setViews for frames generation
	// since there is a discrepancy between the HIDL APIs and core lib APIs.
	vector<vector<float>> matrix;
	matrix.resize(4);
	for (int i=0; i<4; i++) {
	matrix[i].resize(4);
	}

	while(true) {
	{
	scoped_lock<mutex> lock(mAccessLock);

	if (mStreamState != RUNNING) {
	// Break out of our main thread loop
	LOG(INFO) << "StreamState does not equal to RUNNING. "
	<< "Exiting the loop";
	break;
	}

	if (mOutputWidth != mConfig.width
	\|\| mOutputHeight != mConfig.height) {
	LOG(DEBUG) << "Config changed. Re-allocate memory. "
	<< "Old width: "
	<< mOutputWidth
	<< ", old height: "
	<< mOutputHeight
	<< "; New width: "
	<< mConfig.width
	<< ", new height: "
	<< mConfig.height;
	delete[] static_cast<char*>(mOutputPointer.data_pointer);
	mOutputWidth = mConfig.width;
	mOutputHeight = mConfig.height;
	mOutputPointer.height = mOutputHeight;
	mOutputPointer.width = mOutputWidth;
	mOutputPointer.format = Format::RGBA;
	mOutputPointer.data_pointer =
	new char[mOutputHeight * mOutputWidth * kNumChannels];

	if (!mOutputPointer.data_pointer) {
	LOG(ERROR) << "Memory allocation failed. Exiting.";
	break;
	}

	Size2dInteger size = Size2dInteger(mOutputWidth, mOutputHeight);
	mSurroundView->Update3dOutputResolution(size);

	mSvTexture = new GraphicBuffer(mOutputWidth,
	mOutputHeight,
	HAL_PIXEL_FORMAT_RGBA_8888,
	1,
	GRALLOC_USAGE_HW_TEXTURE,
	"SvTexture");
	if (mSvTexture->initCheck() == OK) {
	LOG(INFO) << "Successfully allocated Graphic Buffer";
	} else {
	LOG(ERROR) << "Failed to allocate Graphic Buffer";
	break;
	}
	}
	}

	// TODO(b/150412555): use hard-coded views for now. Change view every 10
	// frames.
	int recViewId = sequenceId / 10 % 16;
	for (int i=0; i<4; i++)
	for (int j=0; j<4; j++) {
	matrix[i][j] = kRecViews[recViewId][i*4+j];
	}

	if (mSurroundView->Get3dSurroundView(
	mInputPointers, matrix, &mOutputPointer)) {
	LOG(INFO) << "Get3dSurroundView succeeded";
	} else {
	LOG(ERROR) << "Get3dSurroundView failed. "
	<< "Using memset to initialize to gray.";
	memset(mOutputPointer.data_pointer, kGrayColor,
	mOutputHeight * mOutputWidth * kNumChannels);
	}

	void* textureDataPtr = nullptr;
	mSvTexture->lock(GRALLOC_USAGE_SW_WRITE_OFTEN
	\| GRALLOC_USAGE_SW_READ_NEVER,
	&textureDataPtr);
	if (!textureDataPtr) {
	LOG(ERROR) << "Failed to gain write access to GraphicBuffer!";
	break;
	}

	// Note: there is a chance that the stride of the texture is not the
	// same as the width. For example, when the input frame is 1920 * 1080,
	// the width is 1080, but the stride is 2048. So we'd better copy the
	// data line by line, instead of single memcpy.
	uint8_t* writePtr = static_cast<uint8_t*>(textureDataPtr);
	uint8_t* readPtr = static_cast<uint8_t*>(mOutputPointer.data_pointer);
	const int readStride = mOutputWidth * kNumChannels;
	const int writeStride = mSvTexture->getStride() * kNumChannels;
	if (readStride == writeStride) {
	memcpy(writePtr, readPtr, readStride * mSvTexture->getHeight());
	} else {
	for (int i=0; i<mSvTexture->getHeight(); i++) {
	memcpy(writePtr, readPtr, readStride);
	writePtr = writePtr + writeStride;
	readPtr = readPtr + readStride;
	}
	}
	LOG(INFO) << "memcpy finished!";
	mSvTexture->unlock();

	ANativeWindowBuffer* buffer = mSvTexture->getNativeBuffer();
	LOG(DEBUG) << "ANativeWindowBuffer->handle: " << buffer->handle;

	framesRecord.frames.svBuffers.resize(1);
	SvBuffer& svBuffer = framesRecord.frames.svBuffers[0];
	svBuffer.viewId = 0;
	svBuffer.hardwareBuffer.nativeHandle = buffer->handle;
	AHardwareBuffer_Desc* pDesc =
	reinterpret_cast<AHardwareBuffer_Desc *>(
	&svBuffer.hardwareBuffer.description);
	pDesc->width = mOutputWidth;
	pDesc->height = mOutputHeight;
	pDesc->layers = 1;
	pDesc->usage = GRALLOC_USAGE_HW_TEXTURE;
	pDesc->stride = mSvTexture->getStride();
	pDesc->format = HAL_PIXEL_FORMAT_RGBA_8888;
	framesRecord.frames.timestampNs = elapsedRealtimeNano();
	framesRecord.frames.sequenceId = sequenceId++;

	{
	scoped_lock<mutex> lock(mAccessLock);

	if (framesRecord.inUse) {
	LOG(DEBUG) << "Notify SvEvent::FRAME_DROPPED";
	mStream->notify(SvEvent::FRAME_DROPPED);
	} else {
	framesRecord.inUse = true;
	mStream->receiveFrames(framesRecord.frames);
	}
	}
	}

	// If we've been asked to stop, send an event to signal the actual end of stream
	LOG(DEBUG) << "Notify SvEvent::STREAM_STOPPED";
	mStream->notify(SvEvent::STREAM_STOPPED);
	}

	bool SurroundView3dSession::initialize() {
	lock_guard<mutex> lock(mAccessLock, adopt_lock);

	// TODO(b/150412555): ask core-lib team to add API description for "create"
	// method in the .h file.
	// The create method will never return a null pointer based the API
	// description.
	mSurroundView = unique_ptr<SurroundView>(Create());

	mSurroundView->SetStaticData(GetCameras(), Get2dParams(), Get3dParams(),
	GetUndistortionScales(), GetBoundingBox());

	// TODO(b/150412555): remove after EVS camera is used
	mInputPointers = mSurroundView->ReadImages(
	"/etc/automotive/sv/cam0.png",
	"/etc/automotive/sv/cam1.png",
	"/etc/automotive/sv/cam2.png",
	"/etc/automotive/sv/cam3.png");
	if (mInputPointers.size() == 4
	&& mInputPointers[0].cpu_data_pointer != nullptr) {
	LOG(INFO) << "ReadImages succeeded";
	} else {
	LOG(ERROR) << "Failed to read images";
	return false;
	}

	mOutputWidth = Get3dParams().resolution.width;
	mOutputHeight = Get3dParams().resolution.height;

	mConfig.width = mOutputWidth;
	mConfig.height = mOutputHeight;
	mConfig.carDetails = SvQuality::HIGH;

	mOutputPointer.height = mOutputHeight;
	mOutputPointer.width = mOutputWidth;
	mOutputPointer.format = Format::RGBA;
	mOutputPointer.data_pointer = new char[
	mOutputHeight * mOutputWidth * kNumChannels];

	if (!mOutputPointer.data_pointer) {
	LOG(ERROR) << "Memory allocation failed. Exiting.";
	return false;
	}

	mSvTexture = new GraphicBuffer(mOutputWidth,
	mOutputHeight,
	HAL_PIXEL_FORMAT_RGBA_8888,
	1,
	GRALLOC_USAGE_HW_TEXTURE,
	"SvTexture");

	if (mSvTexture->initCheck() == OK) {
	LOG(INFO) << "Successfully allocated Graphic Buffer";
	} else {
	LOG(ERROR) << "Failed to allocate Graphic Buffer";
	return false;
	}

	if (mSurroundView->Start3dPipeline()) {
	LOG(INFO) << "Start3dPipeline succeeded";
	} else {
	LOG(ERROR) << "Start3dPipeline failed";
	return false;
	}

	mIsInitialized = true;
	return true;
	}

	} // namespace implementation
	} // namespace V1_0
	} // namespace sv
	} // namespace automotive
	} // namespace hardware
	} // namespace android