Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add camera offset and use it for CAM log message #5840

Open
roque-canales opened this issue Mar 8, 2017 · 35 comments

Comments

@roque-canales
Copy link

commented Mar 8, 2017

for Copter 3.5 and RTK, it will be very useful to generate real position of camera by adding offset x y z parameter for CAM beetween gps antenna centroid and camera sensor.

Like this position of camera will be correct when multirotor lean angle.

@roque-canales roque-canales changed the title AC 3.5 CAM Message enhancement AC 3.5 improving CAM Message Mar 8, 2017
@mboland

This comment has been minimized.

Copy link

commented Mar 8, 2017

+1 on that
And can we have sub 1 second time stamps?

@OXINARF OXINARF changed the title AC 3.5 improving CAM Message Add camera offset and use it for CAM log message Mar 9, 2017
@ptsneves

This comment has been minimized.

Copy link

commented Mar 10, 2017

@OXINARF , if I provide a patch for this will you accept the addition of this parameters?

@ThorstenDP

This comment has been minimized.

Copy link

commented Mar 10, 2017

+1 and another +1 if we can get the "true" RelAlt above Home logged #4468

@OXINARF

This comment has been minimized.

Copy link
Member

commented Mar 10, 2017

@ptsneves I would probably ask for others' opinion (as I don't use the cam functionality myself) but this seems worth it to me.

@rmackay9

This comment has been minimized.

Copy link
Contributor

commented Mar 11, 2017

In reality I think the timing difference between when the camera message is sent and when the picture is actually taken has a much larger impact on the accuracy of the reported location than the normally small position offset on the vehicle.

Still, adding offset would be consistent with what we've done for other sensors (GPS, optical flow, precision landing camera) so I'm not against it. One question though - should it be in the mount library instead of the camera library or are we talking about a hard mounted cameras only?

@jmachuca77

This comment has been minimized.

Copy link
Contributor

commented Mar 11, 2017

Its not as simple as just adding an offset to the position. The angle of the vehicle at the time the picture is taken has to be taken into consideration to obtain the correct position, this is called lever arm correction. If this is also contained in the PR then it would be a great addition. If the PR only adds an offset then It will not be accurate.

@ptsneves

This comment has been minimized.

Copy link

commented Mar 11, 2017

Well the arm correction makes sense on the mount.
So adding offset and angles. The calculation of the arm is given by for instance tg(a)*offset_x. What is the angle convention here and the referential we should use?

@ThorstenDP

This comment has been minimized.

Copy link

commented Mar 11, 2017

Using a stabilized gimbal the camera position has a continuoisly changing lever arm. But anyway it might help here as well since we have a better estimate of the average camera position.
I agree with Randy that the larger impact comes from the shutter delay. But if we have a precise hotshoe feedback (Trigger + CAM messages) and use RTK a camera offset would be really nice to have.

@ptsneves

This comment has been minimized.

Copy link

commented Mar 11, 2017

How you do have a continuously changing lever arm if you take into account the pitch roll and yaw angles? Do you have telescopic mounts?

@roque-canales

This comment has been minimized.

Copy link
Author

commented Mar 11, 2017

Between RTK antenna gps centroid and camera, on most multicopter frame rtk antenna is above the CG of drone easily 10-15 centimeter, and camera is easily 10-15 centimeters below.

So with rtk + hot shoe, we can have centimetric position of antenna, but no of camera.
Adding x,y,z camera offset for trigonometric processing in live it with roll pitch yaw of multicopter frame, permit to maintain centimetric camera positions.

If not, with lean angle of 25° we add easily 10-15cm of camera position error, witch disable totally the interest of using rtk for georef images.

@ThorstenDP

This comment has been minimized.

Copy link

commented Mar 11, 2017

@roque-canales yes right! But 10-15 cm is good enough for many applications and it is only possible with RTK/PPK as well. But then the camera offset is not crucial.
Not, sure but can we get the angles of the BGC? Maybe from Strom32?

@roque-canales

This comment has been minimized.

Copy link
Author

commented Mar 11, 2017

First resolve this for fixed gimbals. so only offset between rtk antenna and camera + roll pitch yaw are necessary to determinate real camera positions.

Second, if we use 2 axis stabilised gimbal, normaly for survey we put it in nadiral orientation. (-90).
So camera is always facing earth.

Also you can determine gimbal pitch orientation reading RC input value assigned to pitch and scaling it by reading MNT_RC_IN_TILT / MNT_ANGMIN_TIL / MNT_ANGMAX_TIL / RCx_MIN / RCx_MIN

@ThorstenDP

This comment has been minimized.

Copy link

commented Mar 11, 2017

That's true! Depending on the camera there will be a difference between the COG of the gimbal and the lens but this is usually < 5cm.

@OXINARF

This comment has been minimized.

Copy link
Member

commented Mar 12, 2017

If we over complicate it gets harder to get merged. I think that accounting for the vehicle attitude is essential (as otherwise the offset won't have much value), but I think that a gimbal won't change the position that much to be essential to get done now.

@ptsneves

This comment has been minimized.

Copy link

commented Mar 12, 2017

See this commit branch from master for this issue.
I have a patch where CAM messages now have the offsets of the mount included in the location calculation.

I have made a basic patch for ArduCopter and ArduPlane, but I am unhappy with the architecture of the new system.
In this new patch I added the offset parameters to the mount but it seems the mount is not very connected to the AP_Camera or camera messages and it results in an awkward system.
I have not tested this, it is just a proof of concept and compiles in ArduCopter and Arduplane.

I did not open a pull request yet because this code is not ready for merge. Let me know what you think.

@ptsneves

This comment has been minimized.

Copy link

commented Mar 12, 2017

Another question is why is the AP_Camera not related to AP_Mount at all. A mount can have different things beside a picture camera but a picture camera for sure needs a mount, even if only a conceptual static mount, but one which has position (the optical center of the camera for correctly designed mounts).

This mess leads to the fact that my patch is incompatible with AP_Camera being enabled and AP_Mount disabled.

Another result of this mess is that the send_feedback method of AP_Camera now would require an AP_Mount& argument to be able to send by mavlink the same that is recorded to the log.

@WickedShell

This comment has been minimized.

Copy link
Contributor

commented Mar 12, 2017

@ptsneves You are missing altitude adjustments from pitch, and I don't see you actually apply R/P/Y corrections to the data anywhere there? location_offset() only translates the provided location by a number of meters, you need to externally sort out the effect of the attitude on this translation.

Your second _offset group is missing the 2 prepended to the parameter name, which means you have 2 parameters with the same name which isn't good, thats an easy fix though :)

Handling a lack of mount can be done by passing a null pointer and managing that where you calculate the offsets. Otherwise I suppose you could make an offset for a static mount.

@OXINARF

This comment has been minimized.

Copy link
Member

commented Mar 13, 2017

I don't think this should be in the AP_Mount library but instead in the AP_Camera one - it is defining the position of the camera, not the mount.

Another question is why is the AP_Camera not related to AP_Mount at all

AP_Mount is probably misleading, it should be named AP_Gimbal, that's what it is for.

@ptsneves

This comment has been minimized.

Copy link

commented Mar 13, 2017

@WickedShell Yes I have not finished the patch. The lady does not allow for much work on weekends. The attitude will be adjusted the same way as commented in libraries/AP_Mount/AP_Mount.cpp@539. The remaining angles will follow the same logic (I just need to verify the angle and referential convention).

Regarding the 2 offset groups with the same name, this was a good spot. I will fix it. On the topic, where on the code is the second mount used?

Your solution for lack of mount is also the most elegant given the situation. To const pointer we go!

@OXINARF I think this logic should not be in AP_Camera. As I said a Camera always is fixed on a mount even if a static/conceptual one. The current AP_Camera being decoupled from AP_Mount is something to be fixed, not create further wrong designs. Even Randy seems to point to all mounted cameras, whether hard mounted or not. If the patch will not be accepted because of this I will not re-factor it and keep it in my fork, even though I would be disappointed.

I will try to update the patch set today.

@OXINARF

This comment has been minimized.

Copy link
Member

commented Mar 14, 2017

As I said a Camera always is fixed on a mount even if a static/conceptual one.

Yes, but a static mount has no code to go with it, it's invisible to the autopilot.

The current AP_Camera being decoupled from AP_Mount is something to be fixed, not create further wrong designs.

That's your opinion. It's not wrong in my opinion. You can have a camera without a gimbal and you can have a gimbal without a camera.

If the patch will not be accepted because of this I will not re-factor it and keep it in my fork, even though I would be disappointed.

I wouldn't accept it, but others might. The proof to me that this is wrong is the simple fact that you are adding two sets of parameters (and methods that work with them), but you only use one of them since the AP_Camera library only supports one instance - and that shows this is AP_Camera related, not AP_Mount.

@ptsneves

This comment has been minimized.

Copy link

commented Mar 14, 2017

@OXINARF By this order of ideas the frame type of a quadcopter would also be invisible to the Autopilot. After all it is just a static frame. Regardless of the approach, if you come up with an alternative implementation I do not think choice would be a tragedy ;)

Back on topic:

Matrix3f vehicle_dcm;
vehicle_dcm.from_euler(ahrs.roll, ahrs.pitch, ahrs.yaw);

Vector3f rotated_offsets = vehicle_dcm * offsets; 
location_offset(mount_location, rotated_offsets.x, rotated_offsets.y);

Is this the correct code to rotate the offset to a body centered coordinates?

@ptsneves

This comment has been minimized.

Copy link

commented Mar 14, 2017

New commit with angles and refactor to use the AP_Camera instead of the AP_Mount. The way the ifdefs are set forces the usage of AP_Camera.
https://github.com/ptsneves/ardupilot/tree/camera_offset

@OXINARF

This comment has been minimized.

Copy link
Member

commented Mar 15, 2017

By this order of ideas the frame type of a quadcopter would also be invisible to the Autopilot. After all it is just a static frame

In a sense it is. We only need to know how many motors you have and where they are because of physics - but then all the code is the same for a quad, a hex or an octa. But where your comparison fails is that the frame isn't static, it's moving in the air, if it was static you wouldn't need an autopilot 😃 A static mount on the other hand isn't moving (the frame is) so it doesn't matter to the autopilot.

Anyway, new code looks better to me - a couple of minor issues here and there, but very easily fixable. Regarding the offset math I'm not the person to comment on it. @WickedShell can you help here?

@ptsneves

This comment has been minimized.

Copy link

commented Mar 15, 2017

@OXINARF You have a point. Can you point out the minor issues?
@WickedShell Can you give a look at the math code?

Also I have updated the branch to use the offsets in the mavlink message. I have not added this offset calculation to the update_location because the navigation is done in terms of cg position and with a trigger based on offsets, this could create unreachable triggers.

@WickedShell

This comment has been minimized.

Copy link
Contributor

commented Mar 15, 2017

@ptsneves I don't really have time to work out the correct math at moment, but the proposed math isn't correct, as it doesn't apply any altitude compensation (IE from pitching moments).

@ptsneves

This comment has been minimized.

Copy link

commented Mar 15, 2017

@WickedShell :I think you are referring the my first patch. My recent one uses the dcm matrix from the yaw,pitch and roll to rotate the point. Can you confirm this does not compensate for the pitching moments?

@WickedShell

This comment has been minimized.

Copy link
Contributor

commented Mar 15, 2017

I'm not prepared to comment on the DCM part, but you never actually touch the altitude when you do the corrections.

@ptsneves

This comment has been minimized.

Copy link

commented Mar 15, 2017

@WickedShell Dhough you are right. I was thinking that the rotation matrix was enough but this only calculates the correct components. I adde the offsets to the horizontal coordinates but forgot adding the z component.
I already pushed the fix.
Is there anyway I can validate the math?

@OXINARF

This comment has been minimized.

Copy link
Member

commented Mar 16, 2017

@ptsneves I've commented on your commits with the minor issues I've seen. If you aren't certain of the math, the best is to open a PR and I'll try to get someone to review.

@roque-canales

This comment has been minimized.

Copy link
Author

commented May 4, 2017

Does someone pushed this into master for it be released with the next beta for we test it?

@ptsneves

This comment has been minimized.

Copy link

commented May 4, 2017

@roque-canales It was my fault. I forgot to open the pull request as OXINARF requested. I will try to do that today.

@roque-canales

This comment has been minimized.

Copy link
Author

commented May 13, 2017

@ptsneves don't worry is just that 3.5 rc6 will be release in the next few days and maybe it can be added in ?

@roque-canales

This comment has been minimized.

Copy link
Author

commented Jun 10, 2017

Hello Randy,

Could you please merge this PR at master for it be added to 3.5 release?
#6266

@roque-canales

This comment has been minimized.

Copy link
Author

commented Jan 6, 2018

Hello, there is update on this feature? It will be avalable for test in next beta release?

@norimboo

This comment has been minimized.

Copy link

commented Mar 5, 2019

Hi what is happening with this update ? When it will be released ?

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
9 participants
You can’t perform that action at this time.