void
SequenceCursor::removeTrack (const SequencerTrack* track)
{
const int index = track->index();
if (index >= 0 && uint32_t (index) < numTracks())
removeTrack ((uint32_t) index);
}
void
SequenceCursor::removeTrack (int32 track)
{
if (trackCount.set (trackCount.get() - 1))
{
for ( ; track < numTracks(); ++track)
rtClips [track] = rtClips [track + 1];
rtClips [track] = nullptr;
}
}
void K3b::Device::DiskInfo::debug() const
{
kDebug() << "DiskInfo:" << endl
<< "Mediatype: " << K3b::Device::mediaTypeString( mediaType() ) << endl
<< "Current Profile: " << K3b::Device::mediaTypeString( currentProfile() ) << endl
<< "Disk state: " << ( diskState() == K3b::Device::STATE_EMPTY ?
"empty" :
( diskState() == K3b::Device::STATE_INCOMPLETE ?
"incomplete" :
( diskState() == K3b::Device::STATE_COMPLETE ?
"complete" :
( diskState() == K3b::Device::STATE_NO_MEDIA ?
"no media" :
"unknown" ) ) ) ) << endl
<< "Empty: " << empty() << endl
<< "Rewritable: " << rewritable() << endl
<< "Appendable: " << appendable() << endl
<< "Sessions: " << numSessions() << endl
<< "Tracks: " << numTracks() << endl
<< "Layers: " << numLayers() << endl
<< "Capacity: " << capacity()
<< " (LBA " << capacity().lba()
<< ") (" << capacity().mode1Bytes() << " Bytes)" << endl
<< "Remaining size: " << remainingSize()
<< " (LBA " << remainingSize().lba()
<< ") (" << remainingSize().mode1Bytes() << " Bytes)" << endl
<< "Used Size: " << size()
<< " (LBA " << size().lba()
<< ") (" << size().mode1Bytes() << " Bytes)" << endl;
if( mediaType() == K3b::Device::MEDIA_DVD_PLUS_RW )
kDebug() << "Bg Format: " << ( bgFormatState() == BG_FORMAT_NONE ?
"none" :
( bgFormatState() == BG_FORMAT_INCOMPLETE ?
"incomplete" :
( bgFormatState() == BG_FORMAT_IN_PROGRESS ?
"in progress" :
( bgFormatState() == BG_FORMAT_COMPLETE ?
"complete" : "unknown" ) ) ) ) << endl;
}
void PCTrackContainer::toPointCloudXYZI(Cloud &cloudOut)
{
for(int i=0;i<numTracks();i++){
if(tracks.at(i)->lastFrame().time == currentT){
PCObject object;
object = tracks.at(i)->lastFrame().object;
int id = tracks.at(i)->id;
for(int j=0;j<object.points.size();j++){
PointT point;
point.x = object.points.at(j).pos[0];
point.y = object.points.at(j).pos[1];
point.z = object.points.at(j).pos[2];
point.r = r[id];
point.g = g[id];
point.b = b[id];
// point.intensity = id;
cloudOut.points.push_back(point);
}
}
}
}
void WebmWriter::initStream(size_t idx) {
if (mStreams[idx].mThread != NULL) {
return;
}
if (mStreams[idx].mSource == NULL) {
ALOGV("adding dummy source ... ");
mStreams[idx].mThread = new WebmFrameEmptySourceThread(
mStreams[idx].mType, mStreams[idx].mSink);
} else {
ALOGV("adding source %p", mStreams[idx].mSource.get());
mStreams[idx].mThread = new WebmFrameMediaSourceThread(
mStreams[idx].mSource,
mStreams[idx].mType,
mStreams[idx].mSink,
mTimeCodeScale,
mStartTimestampUs,
mStartTimeOffsetMs,
numTracks(),
mIsRealTimeRecording);
}
}
visualization_msgs::Marker PCTrackContainer::toMarkerEdges()
{
visualization_msgs::Marker edgeMarker;
edgeMarker.header.frame_id = "/origin";
edgeMarker.header.stamp = ros::Time();
edgeMarker.ns = "edge";
edgeMarker.id = 1;
edgeMarker.type = visualization_msgs::Marker::LINE_LIST;
edgeMarker.action = visualization_msgs::Marker::ADD;
edgeMarker.lifetime = ros::Duration(0);
edgeMarker.scale.x = 0.003;
edgeMarker.color.a = 1;
edgeMarker.color.r = 1;
edgeMarker.color.g = 1;
edgeMarker.color.b = 1;
for(int i=0;i<numTracks();i++){
if(tracks.at(i)->lastFrame().time == currentT){
PCObject object;
object = tracks.at(i)->lastFrame().object;
for(int j=0;j<object.edges.size();j++){
Point p = object.edges.at(j).u;
geometry_msgs::Point point;
point.x = p.pos[0];
point.y = p.pos[1];
point.z = p.pos[2];
edgeMarker.points.push_back(point);
p = object.edges.at(j).v;
point.x = p.pos[0];
point.y = p.pos[1];
point.z = p.pos[2];
edgeMarker.points.push_back(point);
}
}
}
return edgeMarker;
}
visualization_msgs::MarkerArray PCTrackContainer::toMarkerGMMs()
{
double margin = 0.1;
double stepsize = 0.01;
visualization_msgs::MarkerArray gmmMarkers;
int cnt = 0;
for(int i=0;i<numTracks();i++){
// if(tracks.at(i)->lastFrame().time == currentT){
PCObject object;
object = tracks.at(i)->lastFrame().object;
int id = tracks.at(i)->id;
// make a gmm eval points of the object
for(int j=0;j<object.gmm.size();j++){
cnt ++;
visualization_msgs::Marker gmmMarker;
gmmMarker.header.frame_id = "/origin";
gmmMarker.header.stamp = ros::Time();
gmmMarker.ns = "gmm";
gmmMarker.id = cnt;
Gaussian gmm = object.gmm.at(j);
gmmMarker.type = visualization_msgs::Marker::SPHERE;
gmmMarker.action = visualization_msgs::Marker::ADD;
gmmMarker.lifetime = ros::Duration(0);
gmmMarker.pose.position.x = gmm.mean[0];
gmmMarker.pose.position.y = gmm.mean[1];
gmmMarker.pose.position.z = gmm.mean[2];
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(gmm.covariance);
Eigen::Vector3d eigenvalues = eigensolver.eigenvalues();
Eigen::Matrix3d eigenvectors = eigensolver.eigenvectors();
Eigen::Matrix3d rotation = eigenvectors;
double r11 = rotation.col(0)[0];
double r21 = rotation.col(0)[1];
double r31 = rotation.col(0)[2];
double r12 = rotation.col(1)[0];
double r22 = rotation.col(1)[1];
double r32 = rotation.col(1)[2];
double r13 = rotation.col(2)[0];
double r23 = rotation.col(2)[1];
double r33 = rotation.col(2)[2];
double q0 = ( r11 + r22 + r33 + 1.0f) / 4.0f;
double q1 = ( r11 - r22 - r33 + 1.0f) / 4.0f;
double q2 = (-r11 + r22 - r33 + 1.0f) / 4.0f;
double q3 = (-r11 - r22 + r33 + 1.0f) / 4.0f;
if(q0 < 0.0f) q0 = 0.0f;
if(q1 < 0.0f) q1 = 0.0f;
if(q2 < 0.0f) q2 = 0.0f;
if(q3 < 0.0f) q3 = 0.0f;
q0 = sqrt(q0);
q1 = sqrt(q1);
q2 = sqrt(q2);
q3 = sqrt(q3);
if(q0 >= q1 && q0 >= q2 && q0 >= q3) {
q0 *= +1.0f;
q1 *= SIGN(r32 - r23);
q2 *= SIGN(r13 - r31);
q3 *= SIGN(r21 - r12);
} else if(q1 >= q0 && q1 >= q2 && q1 >= q3) {
q0 *= SIGN(r32 - r23);
q1 *= +1.0f;
q2 *= SIGN(r21 + r12);
q3 *= SIGN(r13 + r31);
} else if(q2 >= q0 && q2 >= q1 && q2 >= q3) {
q0 *= SIGN(r13 - r31);
q1 *= SIGN(r21 + r12);
q2 *= +1.0f;
q3 *= SIGN(r32 + r23);
} else if(q3 >= q0 && q3 >= q1 && q3 >= q2) {
q0 *= SIGN(r21 - r12);
q1 *= SIGN(r31 + r13);
q2 *= SIGN(r32 + r23);
q3 *= +1.0f;
} else {
printf("coding error\n");
}
float rq = NORM(q0, q1, q2, q3);
q0 /= rq;
q1 /= rq;
q2 /= rq;
q3 /= rq;
gmmMarker.pose.orientation.w = q0;
gmmMarker.pose.orientation.x = q1;
gmmMarker.pose.orientation.y = q2;
gmmMarker.pose.orientation.z = q3;
gmmMarker.frame_locked = 0;
gmmMarker.scale.x = sqrt(eigenvalues[0])*2;
gmmMarker.scale.y = sqrt(eigenvalues[1])*2;
gmmMarker.scale.z = sqrt(eigenvalues[2])*2;
// std::cout<<" scalex "<<gmmMarker.scale.x;
// std::cout<<" scaley "<<gmmMarker.scale.y;
// std::cout<<" scalez "<<gmmMarker.scale.z;
// std::cout<<std::endl;
// gmmMarker.scale.x = 0.02;
// gmmMarker.scale.y = 0.02;
// gmmMarker.scale.z = 0.02;
gmmMarker.color.a = gmm.weight*object.gmm.size()/2;
// gmmMarker.color.a = 1.0;
gmmMarker.color.r = ((double)r[id])/256;
gmmMarker.color.g = ((double)g[id])/256;
gmmMarker.color.b = ((double)b[id])/256;
// gmmMarker.color.r = 1.;
// gmmMarker.color.g = 1.;
// gmmMarker.color.b = 1.;
gmmMarkers.markers.push_back(gmmMarker);
}
// delete old marker
if(cnt < oldCnt){
for(int j=cnt+1;j<=oldCnt;j++){
visualization_msgs::Marker gmmMarker;
gmmMarker.header.frame_id = "/origin";
gmmMarker.header.stamp = ros::Time();
gmmMarker.ns = "gmm";
gmmMarker.id = j;
gmmMarker.type = visualization_msgs::Marker::SPHERE;
gmmMarker.action = visualization_msgs::Marker::DELETE;
gmmMarker.lifetime = ros::Duration(0);
gmmMarkers.markers.push_back(gmmMarker);
}
}
oldCnt = cnt;
// }
}
return gmmMarkers;
}
status_t WebmWriter::reset() {
if (mInitCheck != OK) {
return OK;
} else {
if (!mStarted) {
release();
return OK;
}
}
status_t err = OK;
int64_t maxDurationUs = 0;
int64_t minDurationUs = 0x7fffffffffffffffLL;
for (int i = 0; i < kMaxStreams; ++i) {
if (mStreams[i].mThread == NULL) {
continue;
}
status_t status = mStreams[i].mThread->stop();
if (err == OK && status != OK) {
err = status;
}
int64_t durationUs = mStreams[i].mThread->getDurationUs();
if (durationUs > maxDurationUs) {
maxDurationUs = durationUs;
}
if (durationUs < minDurationUs) {
minDurationUs = durationUs;
}
}
if (numTracks() > 1) {
ALOGD("Duration from tracks range is [%" PRId64 ", %" PRId64 "] us", minDurationUs, maxDurationUs);
}
mSinkThread->stop();
// Do not write out movie header on error.
if (err != OK) {
release();
return err;
}
sp<WebmElement> cues = new WebmMaster(kMkvCues, mCuePoints);
uint64_t cuesSize = cues->totalSize();
// TRICKY Even when the cues do fit in the space we reserved, if they do not fit
// perfectly, we still need to check if there is enough "extra space" to write an
// EBML void element.
if (cuesSize != mEstimatedCuesSize && cuesSize > mEstimatedCuesSize - kMinEbmlVoidSize) {
mCuesOffset = ::lseek(mFd, 0, SEEK_CUR);
cues->write(mFd, cuesSize);
} else {
uint64_t spaceSize;
::lseek(mFd, mCuesOffset, SEEK_SET);
cues->write(mFd, cuesSize);
sp<WebmElement> space = new EbmlVoid(mEstimatedCuesSize - cuesSize);
space->write(mFd, spaceSize);
}
mCuePoints.clear();
mStreams[kVideoIndex].mSink.clear();
mStreams[kAudioIndex].mSink.clear();
uint8_t bary[sizeof(uint64_t)];
uint64_t totalSize = ::lseek(mFd, 0, SEEK_END);
uint64_t segmentSize = totalSize - mSegmentDataStart;
::lseek(mFd, mSegmentOffset + sizeOf(kMkvSegment), SEEK_SET);
uint64_t segmentSizeCoded = encodeUnsigned(segmentSize, sizeOf(kMkvUnknownLength));
serializeCodedUnsigned(segmentSizeCoded, bary);
::write(mFd, bary, sizeOf(kMkvUnknownLength));
uint64_t durationOffset = mInfoOffset + sizeOf(kMkvInfo) + sizeOf(mInfoSize)
+ sizeOf(kMkvSegmentDuration) + sizeOf(sizeof(double));
sp<WebmElement> duration = new WebmFloat(
kMkvSegmentDuration,
(double) (maxDurationUs * 1000 / mTimeCodeScale));
duration->serializePayload(bary);
::lseek(mFd, durationOffset, SEEK_SET);
::write(mFd, bary, sizeof(double));
List<sp<WebmElement> > seekEntries;
seekEntries.push_back(WebmElement::SeekEntry(kMkvInfo, mInfoOffset - mSegmentDataStart));
seekEntries.push_back(WebmElement::SeekEntry(kMkvTracks, mTracksOffset - mSegmentDataStart));
seekEntries.push_back(WebmElement::SeekEntry(kMkvCues, mCuesOffset - mSegmentDataStart));
sp<WebmElement> seekHead = new WebmMaster(kMkvSeekHead, seekEntries);
uint64_t metaSeekSize;
::lseek(mFd, mSegmentDataStart, SEEK_SET);
seekHead->write(mFd, metaSeekSize);
uint64_t spaceSize;
sp<WebmElement> space = new EbmlVoid(kMaxMetaSeekSize - metaSeekSize);
space->write(mFd, spaceSize);
release();
return err;
}
