On 28/09/2017 at 04:15, xxxxxxxx wrote:
Here a sample for doing what you want to achieve in python.
There is small issue regarding coordinate and orientation since it's appear to not be the one describe here http://www.stereolabs.com/documentation/overview/positional-tracking/coordinate-frames.html
So maybe a picture with the actual world axis and the mesh + the track of the camera would be nice.
Moreover I'm not sure, I still need to investigate but I guess my script suffer of a memory leak in AddKey function. Then use it with care.
#Import csv stereolabs inside c4d
# https://plugincafe.maxon.net/topic/659/13815_3d-rgbd-camera--c4d-import
# graphos 28.09.2017
# THIS VERSION ACTUALLY NOT WORK FULLY => DO NOT USE IT IN PRODUCTION
# USAGE:
# Copy - past in Script manager
# Select a camera
# Run
# Select csv file
# Enjoy ! :)
import c4d
import csv
class Data(object) :
timestamp = None
rot_x = None
rot_y = None
rot_z = None
pos_x = None
pos_y = None
pos_z = None
def __init__(self, data) :
if not isinstance(data, str) : return
# Build a list from string
datas = data.replace(" ", "").split(";")
buffer = list()
for i, value in enumerate(datas) :
try:
x = None
if not i:
x = int(value)
else:
x = float(value)
buffer.append(x)
except:
pass
# Check list is correct
if len(buffer) != 7: return
# Assign value
self.timestamp = buffer[0]
self.rot_x = buffer[1]
self.rot_y = buffer[2]
self.rot_z = buffer[3]
self.pos_x = buffer[4]
self.pos_y = buffer[5]
self.pos_z = buffer[6]
def __str__(self) :
return "time: {0}, rot_x: {1}, rot_y: {2}, rot_z: {3}, pos_x: {4}, pos_y: {5}, pos_z: {6}".format(
self.timestamp,
self.rot_x,
self.rot_y,
self.rot_z,
self.pos_x,
self.pos_y,
self.pos_z
)
def _getChildrenWorld(self, obj) :
buffer = list()
children = obj.GetChildren()
for child in children:
buffer.append(child.GetMg())
return buffer
def _setChildrenWorld(self, obj, list_matrice) :
children = obj.GetChildren()
if len(children) != len(list_matrice) : return False
for i, matrice in enumerate(list_matrice) :
doc.AddUndo(c4d.UNDOTYPE_CHANGE, children[i])
children[i].SetMg(matrice)
return True
def setWorldPosition(self, obj) :
m = obj.GetMg()
m.off = c4d.Vector(-self.pos_x, self.pos_y, self.pos_z)
doc.AddUndo(c4d.UNDOTYPE_CHANGE, obj)
obj.SetMg(m)
def setWorldRotation(self, obj) :
old_pos = self._getChildrenWorld(obj)
doc.AddUndo(c4d.UNDOTYPE_CHANGE, obj)
rot = c4d.Vector(-self.rot_x, self.rot_y, self.rot_z)
m = c4d.utils.HPBToMatrix(rot)
m.off = obj.GetMg().off
obj.SetMg(m)
self._setChildrenWorld(obj, old_pos)
def checkValidCSV(filepath) :
if filepath[-3:] != "csv": return False
file = open(filepath, "rb")
try:
line = file.readline()[:-2]
str_to_check = "Timestamp(ns);Rotation_X(rad);Rotation_Y(rad);Rotation_Z(rad);Position_X(m);Position_Y(m);Position_Z(m);"
finally:
file.close()
if line != str_to_check: return False
return True
def getTrack(obj, trackID, channelID) :
# Find the Track
param = c4d.DescID(c4d.DescLevel(trackID, c4d.DTYPE_VECTOR, 0),
c4d.DescLevel(channelID, c4d.DTYPE_REAL, 0)
)
track = obj.FindCTrack(param)
# Create if no track found
if not track:
track = c4d.CTrack(obj, param)
doc.AddUndo(c4d.UNDOTYPE_NEW, track)
obj.InsertTrackSorted(track)
return track
def addKey(obj, track, frame, value) :
# init var
time = c4d.BaseTime(frame, doc.GetFps())
curve = track.GetCurve()
# Find the key
key = curve.FindKey(time)
found = False
# If there is no key create one
if not key:
# init our key
key = c4d.CKey()
track.FillKey(doc, obj, key)
# Set value for our key
key.SetTime(curve, time)
key.SetValue(curve, value)
# Insert our key
doc.AddUndo(c4d.UNDOTYPE_NEW, key)
curve.InsertKey(key)
# If we found an already existant at current time, jsut set the the value.
else:
key = key["key"]
# Set new key value
doc.AddUndo(c4d.UNDOTYPE_CHANGE, key)
key.SetValue(curve, value)
def main() :
# Get selected camera
obj = doc.GetActiveObject()
if not obj or not obj.CheckType(c4d.Ocamera) : return
# Open file and check validity
filepath = c4d.storage.LoadDialog(c4d.FILESELECTTYPE_ANYTHING, "Select csv data")
if not filepath: return
if not checkValidCSV(filepath) : return
# Read data and store it in list(datas)
datas = list()
with open(filepath, "rb") as file:
reader = csv.reader(file)
next(reader)
for row in reader:
datas.append(Data(row[0]))
# Create Keyframe for each data
doc.StartUndo()
# I acutally limit to the first 600 frame since there is memory leak and we can already so rotation are not good
for i, data in enumerate(datas[:600]) :
# Set Frame
doc.SetTime(c4d.BaseTime(i, doc.GetFps()))
c4d.DrawViews(c4d.DRAWFLAGS_FORCEFULLREDRAW)
c4d.GeSyncMessage(c4d.EVMSG_TIMECHANGED)
# Get tracks
posTrackX = getTrack(obj, c4d.ID_BASEOBJECT_POSITION, c4d.VECTOR_X)
posTrackY = getTrack(obj, c4d.ID_BASEOBJECT_POSITION, c4d.VECTOR_Y)
posTrackZ = getTrack(obj, c4d.ID_BASEOBJECT_POSITION, c4d.VECTOR_Z)
rotTrackX = getTrack(obj, c4d.ID_BASEOBJECT_ROTATION, c4d.VECTOR_X)
rotTrackY = getTrack(obj, c4d.ID_BASEOBJECT_ROTATION, c4d.VECTOR_Y)
rotTrackZ = getTrack(obj, c4d.ID_BASEOBJECT_ROTATION, c4d.VECTOR_Z)
# Get world data
current_frame_world_pos = data.setWorldPosition(obj)
current_frame_world_rot = data.setWorldRotation(obj)
# Get relative data
current_frame_local_pos = obj.GetRelPos()
current_frame_local_rot = obj.GetRelRot()
# Add Key for current value
current_frame = doc.GetTime().GetFrame(doc.GetFps())
addKey(obj, posTrackX, current_frame, current_frame_local_pos.x)
addKey(obj, posTrackY, current_frame, current_frame_local_pos.y)
addKey(obj, posTrackZ, current_frame, current_frame_local_pos.z)
addKey(obj, rotTrackX, current_frame, current_frame_local_rot.x)
addKey(obj, rotTrackY, current_frame, current_frame_local_rot.y)
addKey(obj, rotTrackZ, current_frame, current_frame_local_rot.z)
doc.EndUndo()
c4d.EventAdd()
if __name__ == '__main__':
main()
Moreover looking a bit the source you probably use this http://github.com/stereolabs/zed-examples/blob/master/positional tracking/src/main.cpp for create the csv file. Which say
initParameters.coordinate_system = COORDINATE_SYSTEM_RIGHT_HANDED_Y_UP;
But I didn't succes to convert it to good thing inside c4d.
Importing your obj with Flip X Axis enable or Swap X and Z axes give the same result and looking to your video it's seem to be the good orientation, but my matrix mathematics are sadly not good so maybe someone can help.
About timestamp as Riccardo pointed this out, I simply make one line = one frame.